feat: 初始化 Go2 导航相关代码与配置

unitree_sdk2/example/h1/high_level/h1_2_arm_sdk_dds_example.cpp

@@ -0,0 +1,259 @@
+#include <array>
+#include <chrono>
+#include <iostream>
+#include <thread>
+
+#include <unitree/idl/hg/LowCmd_.hpp>
+#include <unitree/idl/hg/LowState_.hpp>
+#include <unitree/robot/channel/channel_publisher.hpp>
+#include <unitree/robot/channel/channel_subscriber.hpp>
+
+static const std::string kTopicArmSDK = "rt/arm_sdk";
+static const std::string kTopicState = "rt/lowstate";
+constexpr float kPi = 3.141592654;
+constexpr float kPi_2 = 1.57079632;
+
+enum JointIndex {
+  // Left leg
+  kLeftHipYaw = 0,
+  kLeftHipPitch = 1,
+  kLeftHipRoll = 2,
+  kLeftKnee = 3,
+  kLeftAnkle = 4,
+  kLeftAnkleRoll = 5,
+  // Right leg
+  kRightHipYaw = 6,
+  kRightHipPitch = 7,
+  kRightHipRoll = 8,
+  kRightKnee = 9,
+  kRightAnkle = 10,
+  kRightAnkleRoll = 11,
+
+  kWaistYaw = 12,
+
+  // Left arm
+  kLeftShoulderPitch = 13,
+  kLeftShoulderRoll = 14,
+  kLeftShoulderYaw = 15,
+  kLeftElbow = 16,
+  kLeftWristRoll = 17,
+  kLeftWristPitch = 18,
+  kLeftWristYaw = 19,
+  // Right arm
+  kRightShoulderPitch = 20,
+  kRightShoulderRoll = 21,
+  kRightShoulderYaw = 22,
+  kRightElbow = 23,
+  kRightWristRoll = 24,
+  kRightWristPitch = 25,
+  kRightWristYaw = 26,
+
+  kNotUsedJoint = 27,
+  kNotUsedJoint1 = 28,
+  kNotUsedJoint2 = 29,
+  kNotUsedJoint3 = 30,
+  kNotUsedJoint4 = 31,
+  kNotUsedJoint5 = 32,
+  kNotUsedJoint6 = 33,
+  kNotUsedJoint7 = 34
+};
+
+int main(int argc, char const *argv[]) {
+  if (argc < 2) {
+    std::cout << "Usage: " << argv[0] << " networkInterface" << std::endl;
+    exit(-1);
+  }
+
+  unitree::robot::ChannelFactory::Instance()->Init(0, argv[1]);
+
+  unitree::robot::ChannelPublisherPtr<unitree_hg::msg::dds_::LowCmd_>
+      arm_sdk_publisher;
+  unitree_hg::msg::dds_::LowCmd_ msg;
+
+  arm_sdk_publisher.reset(
+      new unitree::robot::ChannelPublisher<unitree_hg::msg::dds_::LowCmd_>(
+          kTopicArmSDK));
+  arm_sdk_publisher->InitChannel();
+
+  unitree::robot::ChannelSubscriberPtr<unitree_hg::msg::dds_::LowState_>
+      low_state_subscriber;
+
+  // create subscriber
+  unitree_hg::msg::dds_::LowState_ state_msg;
+  low_state_subscriber.reset(
+      new unitree::robot::ChannelSubscriber<unitree_hg::msg::dds_::LowState_>(
+          kTopicState));
+  low_state_subscriber->InitChannel([&](const void *msg) {
+        auto s = ( const unitree_hg::msg::dds_::LowState_* )msg;
+        memcpy( &state_msg, s, sizeof( unitree_hg::msg::dds_::LowState_ ) );
+  }, 1);
+
+  std::array<JointIndex, 15> arm_joints = {
+      JointIndex::kLeftShoulderPitch,  JointIndex::kLeftShoulderRoll,
+      JointIndex::kLeftShoulderYaw,    JointIndex::kLeftElbow,
+      JointIndex::kLeftWristRoll,       JointIndex::kLeftWristPitch,     JointIndex::kLeftWristYaw,       
+      JointIndex::kRightShoulderPitch, JointIndex::kRightShoulderRoll,
+      JointIndex::kRightShoulderYaw,   JointIndex::kRightElbow, 
+      JointIndex::kRightWristRoll,      JointIndex::kRightWristPitch,    JointIndex::kRightWristYaw,       
+      JointIndex::kWaistYaw};
+
+  float weight = 0.f;
+  float weight_rate = 0.2f;
+
+  std::array<float, 15> kp_array = { 120, 120, 80, 50, 50, 50, 50, 
+                                    120, 120, 80, 50, 50, 50, 50, 
+                                    200 };
+  std::array<float, 15> kd_array = { 2.0, 2.0, 1.5, 1.0, 1.0, 1.0, 1.0, 
+                                    2.0, 2.0, 1.5, 1.0, 1.0, 1.0, 1.0, 
+                                    2.0 };
+  float dq = 0.f;
+  float tau_ff = 0.f;
+
+  float control_dt = 0.02f;
+  float max_joint_velocity = 0.5f;
+
+  float delta_weight = weight_rate * control_dt;
+  float max_joint_delta = max_joint_velocity * control_dt;
+  auto sleep_time =
+      std::chrono::milliseconds(static_cast<int>(control_dt / 0.001f));
+
+  // modify the initial position to avoid collision
+  std::array<float, 15> init_pos{0.f, 0.3,  0.f, 0, 0, 0, 0,
+                                     0.f, -0.3, 0.f, 0, 0, 0, 0,
+                                     0.f};
+
+  std::array<float, 15> target_pos = {0.f, kPi_2,  0.f, kPi_2, 0, 0, 0,
+                                     0.f, -kPi_2, 0.f, kPi_2, 0, 0, 0,
+                                     0.f};
+
+  // wait for init
+  std::cout << "Press ENTER to init arms ...";
+  std::cin.get();
+
+
+  // get current joint position
+  std::array<float, 15> current_jpos{};
+  std::cout<<"Current joint position: ";
+  for (int i = 0; i < arm_joints.size(); ++i) {
+	current_jpos.at(i) = state_msg.motor_state().at(arm_joints.at(i)).q();
+	std::cout << current_jpos.at(i) << " ";
+  }
+  std::cout << std::endl;
+
+  // set init pos
+  std::cout << "Initailizing arms ...";
+  float init_time = 2.0f;
+  int init_time_steps = static_cast<int>(init_time / control_dt);
+
+  for (int i = 0; i < init_time_steps; ++i) {
+    // set weight
+    weight = 1.0;
+    msg.motor_cmd().at(JointIndex::kNotUsedJoint).q(weight);
+    float phase = 1.0 * i / init_time_steps;
+    std::cout << "Phase: " << phase << std::endl;
+
+    // set control joints
+    for (int j = 0; j < init_pos.size(); ++j) {
+      msg.motor_cmd().at(arm_joints.at(j)).q(init_pos.at(j) * phase + current_jpos.at(j) * (1 - phase));
+      msg.motor_cmd().at(arm_joints.at(j)).dq(dq);
+      msg.motor_cmd().at(arm_joints.at(j)).kp(kp_array.at(j));
+      msg.motor_cmd().at(arm_joints.at(j)).kd(kd_array.at(j));
+      msg.motor_cmd().at(arm_joints.at(j)).tau(tau_ff);
+    }
+
+    // send dds msg
+    arm_sdk_publisher->Write(msg);
+
+    // sleep
+    std::this_thread::sleep_for(sleep_time);
+  }
+
+  std::cout << "Done!" << std::endl;
+
+  // wait for control
+  std::cout << "Press ENTER to start arm ctrl ..." << std::endl;
+  std::cin.get();
+
+  // start control
+  std::cout << "Start arm ctrl!" << std::endl;
+  float period = 5.f;
+  int num_time_steps = static_cast<int>(period / control_dt);
+
+  std::array<float, 15> current_jpos_des{0.f, 0.3,  0.f, 0, 0, 0, 0,
+                                        0.f, -0.3, 0.f, 0, 0, 0, 0,
+                                        0.f};
+
+  // lift arms up
+  for (int i = 0; i < num_time_steps; ++i) {
+    // update jpos des
+    for (int j = 0; j < init_pos.size(); ++j) {
+      current_jpos_des.at(j) +=
+          std::clamp(target_pos.at(j) - current_jpos_des.at(j),
+                     -max_joint_delta, max_joint_delta);
+    }
+
+    // set control joints
+    for (int j = 0; j < init_pos.size(); ++j) {
+      msg.motor_cmd().at(arm_joints.at(j)).q(current_jpos_des.at(j));
+      msg.motor_cmd().at(arm_joints.at(j)).dq(dq);
+      msg.motor_cmd().at(arm_joints.at(j)).kp(kp_array.at(j));
+      msg.motor_cmd().at(arm_joints.at(j)).kd(kd_array.at(j));
+      msg.motor_cmd().at(arm_joints.at(j)).tau(tau_ff);
+    }
+
+    // send dds msg
+    arm_sdk_publisher->Write(msg);
+
+    // sleep
+    std::this_thread::sleep_for(sleep_time);
+  }
+
+  // put arms down
+  for (int i = 0; i < num_time_steps; ++i) {
+    // update jpos des
+    for (int j = 0; j < init_pos.size(); ++j) {
+      current_jpos_des.at(j) +=
+          std::clamp(init_pos.at(j) - current_jpos_des.at(j), -max_joint_delta,
+                     max_joint_delta);
+    }
+
+    // set control joints
+    for (int j = 0; j < init_pos.size(); ++j) {
+      msg.motor_cmd().at(arm_joints.at(j)).q(current_jpos_des.at(j));
+      msg.motor_cmd().at(arm_joints.at(j)).dq(dq);
+      msg.motor_cmd().at(arm_joints.at(j)).kp(kp_array.at(j));
+      msg.motor_cmd().at(arm_joints.at(j)).kd(kd_array.at(j));
+      msg.motor_cmd().at(arm_joints.at(j)).tau(tau_ff);
+    }
+
+    // send dds msg
+    arm_sdk_publisher->Write(msg);
+
+    // sleep
+    std::this_thread::sleep_for(sleep_time);
+  }
+
+  // stop control
+  std::cout << "Stoping arm ctrl ...";
+  float stop_time = 2.0f;
+  int stop_time_steps = static_cast<int>(stop_time / control_dt);
+
+  for (int i = 0; i < stop_time_steps; ++i) {
+    // increase weight
+    weight -= delta_weight;
+    weight = std::clamp(weight, 0.f, 1.f);
+
+    // set weight
+    msg.motor_cmd().at(JointIndex::kNotUsedJoint).q(weight);
+
+    // send dds msg
+    arm_sdk_publisher->Write(msg);
+
+    // sleep
+    std::this_thread::sleep_for(sleep_time);
+  }
+
+  std::cout << "Done!" << std::endl;
+
+  return 0;
+}

unitree_sdk2/example/h1/high_level/h1_arm_sdk_dds_example.cpp

@@ -0,0 +1,234 @@
+#include <array>
+#include <chrono>
+#include <iostream>
+#include <thread>
+
+#include <unitree/idl/go2/LowCmd_.hpp>
+#include <unitree/idl/hg/LowState_.hpp>
+#include <unitree/robot/channel/channel_publisher.hpp>
+#include <unitree/robot/channel/channel_subscriber.hpp>
+
+static const std::string kTopicArmSDK = "rt/arm_sdk";
+static const std::string kTopicState = "rt/lowstate";
+constexpr float kPi = 3.141592654;
+constexpr float kPi_2 = 1.57079632;
+
+enum JointIndex {
+  // Right leg
+  kRightHipYaw = 8,
+  kRightHipRoll = 0,
+  kRightHipPitch = 1,
+  kRightKnee = 2,
+  kRightAnkle = 11,
+  // Left leg
+  kLeftHipYaw = 7,
+  kLeftHipRoll = 3,
+  kLeftHipPitch = 4,
+  kLeftKnee = 5,
+  kLeftAnkle = 10,
+
+  kWaistYaw = 6,
+
+  kNotUsedJoint = 9,
+
+  // Right arm
+  kRightShoulderPitch = 12,
+  kRightShoulderRoll = 13,
+  kRightShoulderYaw = 14,
+  kRightElbow = 15,
+  // Left arm
+  kLeftShoulderPitch = 16,
+  kLeftShoulderRoll = 17,
+  kLeftShoulderYaw = 18,
+  kLeftElbow = 19,
+
+};
+
+int main(int argc, char const *argv[]) {
+  if (argc < 2) {
+    std::cout << "Usage: " << argv[0] << " networkInterface" << std::endl;
+    exit(-1);
+  }
+
+  unitree::robot::ChannelFactory::Instance()->Init(0, argv[1]);
+
+  unitree::robot::ChannelPublisherPtr<unitree_go::msg::dds_::LowCmd_>
+      arm_sdk_publisher;
+  unitree_go::msg::dds_::LowCmd_ msg;
+
+  arm_sdk_publisher.reset(
+      new unitree::robot::ChannelPublisher<unitree_go::msg::dds_::LowCmd_>(
+          kTopicArmSDK));
+  arm_sdk_publisher->InitChannel();
+
+  unitree::robot::ChannelSubscriberPtr<unitree_hg::msg::dds_::LowState_>
+      low_state_subscriber;
+
+  // create subscriber
+  unitree_hg::msg::dds_::LowState_ state_msg;
+  low_state_subscriber.reset(
+      new unitree::robot::ChannelSubscriber<unitree_hg::msg::dds_::LowState_>(
+          kTopicState));
+  low_state_subscriber->InitChannel([&](const void *msg) {
+        auto s = ( const unitree_hg::msg::dds_::LowState_* )msg;
+        memcpy( &state_msg, s, sizeof( unitree_hg::msg::dds_::LowState_ ) );
+  }, 1);
+
+  std::array<JointIndex, 9> arm_joints = {
+      JointIndex::kLeftShoulderPitch,  JointIndex::kLeftShoulderRoll,
+      JointIndex::kLeftShoulderYaw,    JointIndex::kLeftElbow,
+      JointIndex::kRightShoulderPitch, JointIndex::kRightShoulderRoll,
+      JointIndex::kRightShoulderYaw,   JointIndex::kRightElbow, JointIndex::kWaistYaw};
+
+  float weight = 0.f;
+  float weight_rate = 0.2f;
+
+  float kp = 60.f;
+  float kd = 1.5f;
+  float dq = 0.f;
+  float tau_ff = 0.f;
+
+  float control_dt = 0.02f;
+  float max_joint_velocity = 0.5f;
+
+  float delta_weight = weight_rate * control_dt;
+  float max_joint_delta = max_joint_velocity * control_dt;
+  auto sleep_time =
+      std::chrono::milliseconds(static_cast<int>(control_dt / 0.001f));
+
+  std::array<float, 9> init_pos{0, 0, 0, 0, 
+                                0, 0, 0, 0,
+                                0};
+
+  std::array<float, 9> target_pos = {0.f, kPi_2,  0.f, kPi_2,
+                                     0.f, -kPi_2, 0.f, kPi_2,
+                                     0.f};
+
+  // wait for init
+  std::cout << "Press ENTER to init arms ...";
+  std::cin.get();
+
+  // get current joint position
+  std::array<float, 9> current_jpos{};
+  std::cout<<"Current joint position: ";
+  for (int i = 0; i < arm_joints.size(); ++i) {
+	current_jpos.at(i) = state_msg.motor_state().at(arm_joints.at(i)).q();
+	std::cout << current_jpos.at(i) << " ";
+  }
+  std::cout << std::endl;
+
+  // set init pos
+  std::cout << "Initailizing arms ...";
+  float init_time = 2.0f;
+  int init_time_steps = static_cast<int>(init_time / control_dt);
+
+  for (int i = 0; i < init_time_steps; ++i) {
+    // set weight
+    weight = 1.0;
+    msg.motor_cmd().at(JointIndex::kNotUsedJoint).q(weight);
+    float phase = 1.0 * i / init_time_steps;
+    std::cout << "Phase: " << phase << std::endl;
+
+    // set control joints
+    for (int j = 0; j < init_pos.size(); ++j) {
+      msg.motor_cmd().at(arm_joints.at(j)).q(init_pos.at(j) * phase + current_jpos.at(j) * (1 - phase));
+      msg.motor_cmd().at(arm_joints.at(j)).dq(dq);
+      msg.motor_cmd().at(arm_joints.at(j)).kp(kp);
+      msg.motor_cmd().at(arm_joints.at(j)).kd(kd);
+      msg.motor_cmd().at(arm_joints.at(j)).tau(tau_ff);
+    }
+
+    // send dds msg
+    arm_sdk_publisher->Write(msg);
+
+    // sleep
+    std::this_thread::sleep_for(sleep_time);
+  }
+
+  std::cout << "Done!" << std::endl;
+
+  // wait for control
+  std::cout << "Press ENTER to start arm ctrl ..." << std::endl;
+  std::cin.get();
+
+  // start control
+  std::cout << "Start arm ctrl!" << std::endl;
+  float period = 5.f;
+  int num_time_steps = static_cast<int>(period / control_dt);
+
+  std::array<float, 9> current_jpos_des{};
+
+  // lift arms up
+  for (int i = 0; i < num_time_steps; ++i) {
+    // update jpos des
+    for (int j = 0; j < init_pos.size(); ++j) {
+      current_jpos_des.at(j) +=
+          std::clamp(target_pos.at(j) - current_jpos_des.at(j),
+                     -max_joint_delta, max_joint_delta);
+    }
+
+    // set control joints
+    for (int j = 0; j < init_pos.size(); ++j) {
+      msg.motor_cmd().at(arm_joints.at(j)).q(current_jpos_des.at(j));
+      msg.motor_cmd().at(arm_joints.at(j)).dq(dq);
+      msg.motor_cmd().at(arm_joints.at(j)).kp(kp);
+      msg.motor_cmd().at(arm_joints.at(j)).kd(kd);
+      msg.motor_cmd().at(arm_joints.at(j)).tau(tau_ff);
+    }
+
+    // send dds msg
+    arm_sdk_publisher->Write(msg);
+
+    // sleep
+    std::this_thread::sleep_for(sleep_time);
+  }
+
+  // put arms down
+  for (int i = 0; i < num_time_steps; ++i) {
+    // update jpos des
+    for (int j = 0; j < init_pos.size(); ++j) {
+      current_jpos_des.at(j) +=
+          std::clamp(init_pos.at(j) - current_jpos_des.at(j), -max_joint_delta,
+                     max_joint_delta);
+    }
+
+    // set control joints
+    for (int j = 0; j < init_pos.size(); ++j) {
+      msg.motor_cmd().at(arm_joints.at(j)).q(current_jpos_des.at(j));
+      msg.motor_cmd().at(arm_joints.at(j)).dq(dq);
+      msg.motor_cmd().at(arm_joints.at(j)).kp(kp);
+      msg.motor_cmd().at(arm_joints.at(j)).kd(kd);
+      msg.motor_cmd().at(arm_joints.at(j)).tau(tau_ff);
+    }
+
+    // send dds msg
+    arm_sdk_publisher->Write(msg);
+
+    // sleep
+    std::this_thread::sleep_for(sleep_time);
+  }
+
+  // stop control
+  std::cout << "Stoping arm ctrl ...";
+  float stop_time = 2.0f;
+  int stop_time_steps = static_cast<int>(stop_time / control_dt);
+
+  for (int i = 0; i < stop_time_steps; ++i) {
+    // increase weight
+    weight -= delta_weight;
+    weight = std::clamp(weight, 0.f, 1.f);
+
+    // set weight
+    msg.motor_cmd().at(JointIndex::kNotUsedJoint).q(weight);
+
+    // send dds msg
+    arm_sdk_publisher->Write(msg);
+
+    // sleep
+    std::this_thread::sleep_for(sleep_time);
+  }
+
+  std::cout << "Done!" << std::endl;
+
+  return 0;
+}

unitree_sdk2/example/h1/high_level/h1_loco_client_example.cpp

@@ -0,0 +1,300 @@
+#include <chrono>
+#include <iostream>
+#include <thread>
+
+#include <unitree/robot/h1/loco/h1_loco_api.hpp>
+#include <unitree/robot/h1/loco/h1_loco_client.hpp>
+
+std::vector<float> stringToFloatVector(const std::string &str) {
+  std::vector<float> result;
+  std::stringstream ss(str);
+  float num;
+  while (ss >> num) {
+    result.push_back(num);
+    // ignore any trailing whitespace
+    ss.ignore();
+  }
+  return result;
+}
+
+int main(int argc, char const *argv[]) {
+  std::map<std::string, std::string> args = {{"network_interface", "lo"}};
+
+  std::map<std::string, std::string> values;
+  for (int i = 1; i < argc; ++i) {
+    std::string arg = argv[i];
+    if (arg.substr(0, 2) == "--") {
+      size_t pos = arg.find("=");
+      std::string key, value;
+      if (pos != std::string::npos) {
+        key = arg.substr(2, pos - 2);
+        value = arg.substr(pos + 1);
+
+        if (value.front() == '"' && value.back() == '"') {
+          value = value.substr(1, value.length() - 2);
+        }
+      } else {
+        key = arg.substr(2);
+        value = "";
+      }
+      if (args.find(key) != args.end()) {
+        args[key] = value;
+      } else {
+        args.insert({{key, value}});
+      }
+    }
+  }
+
+  unitree::robot::ChannelFactory::Instance()->Init(0, args["network_interface"]);
+
+  unitree::robot::h1::LocoClient client;
+
+  client.Init();
+  client.SetTimeout(10.f);
+
+  for (const auto &arg_pair : args) {
+    std::cout << "Processing command: [" << arg_pair.first << "] with param: [" << arg_pair.second << "] ..."
+              << std::endl;
+    if (arg_pair.first == "network_interface") {
+      continue;
+    }
+
+    if (arg_pair.first == "get_fsm_id") {
+      int fsm_id;
+      client.GetFsmId(fsm_id);
+      std::cout << "current fsm_id: " << fsm_id << std::endl;
+    }
+
+    if (arg_pair.first == "get_fsm_mode") {
+      int fsm_mode;
+      client.GetFsmMode(fsm_mode);
+      std::cout << "current fsm_mode: " << fsm_mode << std::endl;
+    }
+
+    if (arg_pair.first == "get_balance_mode") {
+      int balance_mode;
+      client.GetBalanceMode(balance_mode);
+      std::cout << "current balance_mode: " << balance_mode << std::endl;
+    }
+
+    if (arg_pair.first == "get_swing_height") {
+      float swing_height;
+      client.GetSwingHeight(swing_height);
+      std::cout << "current swing_height: " << swing_height << std::endl;
+    }
+
+    if (arg_pair.first == "get_stand_height") {
+      float stand_height;
+      client.GetStandHeight(stand_height);
+      std::cout << "current stand_height: " << stand_height << std::endl;
+    }
+
+    if (arg_pair.first == "get_phase") {
+      std::vector<float> phase;
+      client.GetPhase(phase);
+      std::cout << "current phase: (";
+      for (const auto &p : phase) {
+        std::cout << p << ", ";
+      }
+      std::cout << ")" << std::endl;
+    }
+
+    if (arg_pair.first == "enable_odom") {
+      float x, y, yaw;
+      client.EnableOdom();
+      std::cout << "Send enable odom signal" << std::endl;
+    }
+
+    if (arg_pair.first == "disable_odom") {
+      float x, y, yaw;
+      client.DisableOdom();
+      std::cout << "Send disable odom signal" << std::endl;
+    }
+
+    if (arg_pair.first == "get_odom") {
+      float x, y, yaw;
+      client.GetOdom(x, y, yaw);
+      std::cout << "Get Odom: (" << x << ", " << y << ", " << yaw << ")" << std::endl;
+    }
+
+    if (arg_pair.first == "set_fsm_id") {
+      int fsm_id = std::stoi(arg_pair.second);
+      client.SetFsmId(fsm_id);
+      std::cout << "set fsm_id to " << fsm_id << std::endl;
+    }
+
+    if (arg_pair.first == "set_balance_mode") {
+      int balance_mode = std::stoi(arg_pair.second);
+      client.SetBalanceMode(balance_mode);
+      std::cout << "set balance_mode to " << balance_mode << std::endl;
+    }
+
+    if (arg_pair.first == "set_swing_height") {
+      float swing_height = std::stof(arg_pair.second);
+      client.SetSwingHeight(swing_height);
+      std::cout << "set swing_height to " << swing_height << std::endl;
+    }
+
+    if (arg_pair.first == "set_stand_height") {
+      float stand_height = std::stof(arg_pair.second);
+      client.SetStandHeight(stand_height);
+      std::cout << "set stand_height to " << stand_height << std::endl;
+    }
+
+    if (arg_pair.first == "set_velocity") {
+      std::vector<float> param = stringToFloatVector(arg_pair.second);
+      auto param_size = param.size();
+      float vx, vy, omega, duration;
+      if (param_size == 3) {
+        vx = param.at(0);
+        vy = param.at(1);
+        omega = param.at(2);
+        duration = 1.f;
+      } else if (param_size == 4) {
+        vx = param.at(0);
+        vy = param.at(1);
+        omega = param.at(2);
+        duration = param.at(3);
+      } else {
+        std::cerr << "Invalid param size for method SetVelocity: " << param_size << std::endl;
+        return 1;
+      }
+
+      client.SetVelocity(vx, vy, omega, duration);
+      std::cout << "set velocity to " << arg_pair.second << std::endl;
+    }
+
+    if (arg_pair.first == "set_phase") {
+      std::vector<float> param = stringToFloatVector(arg_pair.second);
+      auto param_size = param.size();
+      if (param_size == 2) {
+        client.SetPhase(param);
+        std::cout << "set phase to " << arg_pair.second << std::endl;
+      } else {
+        std::cerr << "Invalid param size for method SetPhase: " << param_size << std::endl;
+        return 1;
+      }
+    }
+
+    if (arg_pair.first == "set_target_pos") {
+      std::vector<float> param = stringToFloatVector(arg_pair.second);
+      client.SetTargetPos(param.at(0), param.at(1), param.at(2), false);
+      std::cout << "set_target_pos: " << arg_pair.second << std::endl;
+    }
+
+    if (arg_pair.first == "set_target_pos_relative") {
+      std::vector<float> param = stringToFloatVector(arg_pair.second);
+      client.SetTargetPos(param.at(0), param.at(1), param.at(2));
+      std::cout << "set_target_pos: " << arg_pair.second << std::endl;
+    }
+
+    if (arg_pair.first == "damp") {
+      client.Damp();
+    }
+
+    if (arg_pair.first == "start") {
+      client.Start();
+    }
+
+    if (arg_pair.first == "stand_up") {
+      client.StandUp();
+    }
+
+    if (arg_pair.first == "zero_torque") {
+      client.ZeroTorque();
+    }
+
+    if (arg_pair.first == "stop_move") {
+      client.StopMove();
+    }
+
+    if (arg_pair.first == "high_stand") {
+      client.HighStand();
+    }
+
+    if (arg_pair.first == "low_stand") {
+      client.LowStand();
+    }
+
+    if (arg_pair.first == "balance_stand") {
+      client.BalanceStand();
+    }
+
+    if (arg_pair.first == "continous_gait") {
+      bool flag;
+      if (arg_pair.second == "true") {
+        flag = true;
+      } else if (arg_pair.second == "false") {
+        flag = false;
+      } else {
+        std::cerr << "invalid argument: " << arg_pair.second << std::endl;
+        return 1;
+      }
+      client.ContinuousGait(flag);
+    }
+
+    if (arg_pair.first == "switch_move_mode") {
+      bool flag;
+      if (arg_pair.second == "true") {
+        flag = true;
+      } else if (arg_pair.second == "false") {
+        flag = false;
+      } else {
+        std::cerr << "invalid argument: " << arg_pair.second << std::endl;
+        return 1;
+      }
+      client.SwitchMoveMode(flag);
+    }
+
+    if (arg_pair.first == "move") {
+      std::vector<float> param = stringToFloatVector(arg_pair.second);
+      auto param_size = param.size();
+      float vx, vy, omega;
+      if (param_size == 3) {
+        vx = param.at(0);
+        vy = param.at(1);
+        omega = param.at(2);
+      } else {
+        std::cerr << "Invalid param size for method SetVelocity: " << param_size << std::endl;
+        return 1;
+      }
+      client.Move(vx, vy, omega);
+    }
+
+    if (arg_pair.first == "set_next_foot") {
+      bool flag;
+      if (arg_pair.second == "0") {
+        flag = true;
+      } else if (arg_pair.second == "1") {
+        flag = false;
+      } else {
+        std::cerr << "invalid argument: " << arg_pair.second << std::endl;
+        return 1;
+      }
+      client.SetNextFoot(flag);
+    }
+
+    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;
+    }
+
+    std::cout << "Done!" << std::endl;
+  }
+
+  return 0;
+}