initial release

.vscode/settings.json

@@ -0,0 +1,3 @@
+{
+    "ros.distro": "noetic"
+}

CHANGELOG.md

@@ -0,0 +1,5 @@
+# Changelog
+
+## 1.0.1
+
++ Initial release.

CMakeLists.txt

@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 3.10)
+project(h1_inspire_service VERSION 0.1.0)
+
+if(NOT CMAKE_BUILD_TYPE)
+  set(CMAKE_BUILD_TYPE Debug)
+endif()
+
+if (CMAKE_BUILD_TYPE MATCHES "Debug")
+  message("Debug mode")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17 -g -O0 -fPIC")
+else()
+  message("Release mode")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17 -O3 -DNDEBUG -fPIC")
+endif()
+
+find_package(Boost COMPONENTS program_options REQUIRED)
+
+include_directories(
+  /usr/local/include/ddscxx
+  /usr/local/include/iceoryx/v2.0.2
+  include
+)
+
+link_libraries(unitree_sdk2 ddsc ddscxx rt pthread)
+link_libraries(unitree_idl libboost_program_options.a)
+
+add_executable(inspire_hand inspire_ctrl.cpp)
+
+# example
+add_executable(h1_hand_example example/inspire_hand.cpp)

doc/Inspire-Hand.md

@@ -0,0 +1,80 @@
+## Dexterous Hand
+
+Unitree H1 can be equipped with [Inspire Robotics](https://inspire-robots.com/product/frwz/)'s dexterous hand, which has **six degrees of freedom** and 12 motion joints to mimic the human hand for complex movements.
+
+![RH56](./img/RH56.png)
+
+## 
+
+The user can control the hand by publish the **"unitree_go::msg::dds::MotorCmds_"** message to the topic **"rt/inspire/cmd"**, and get the hand state by subscribe the **"unitree_go::msg::dds::MotorStates_"** message from the topic **"rt/inspire/state"**.
+
+```mermaid
+graph LR
+A(user) --rt/inspire/cmd--> B(H1)
+B --rt/inspire/state--> A
+```
+
++ IDL Message Type
+
+Motor data in array format, containing 12 motor data for both hands.
+
+!!!note
+Currently the dexterous hand only supports joint control, i.e. only the parameter q makes sense in the idl format. The others are reserved.
+!!!
+
+
+```bash
+# namespace unitree_go::msg::dds_
+
+# unitree_go::msg::dds_::MotorCmds_
+struct MotorCmds_ 
+{
+    sequence<unitree_go::msg::dds_::MotorCmd_> cmds;
+};
+
+# unitree_go::msg::dds_::MotorStates_
+struct MotorCmds_ 
+{
+    sequence<unitree_go::msg::dds_::MotorState> states;
+};
+```
++ Joint Motor Sequence
+  
+<div style="text-align: center;">
+<table border="1">
+  <tr>
+    <td>Id</td>
+    <td>0</td>
+    <td>1</td>
+    <td>2</td>
+    <td>3</td>
+    <td>4</td>
+    <td>5</td>
+    <td>6</td>
+    <td>7</td>
+    <td>8</td>
+    <td>9</td>
+    <td>10</td>
+    <td>11</td>
+  </tr>
+  <tr>
+    <td rowspan="2">Joint</td>
+    <td colspan="6">Right Hand</td>
+    <td colspan="6">Left Hand</td>
+  </tr>
+  <tr>
+    <td>pinky</td>
+    <td>ring</td>
+    <td>middle</td>
+    <td>index</td>
+    <td>thumb-bend</td>
+    <td>thumb-rotation</td>
+    <td>pinky</td>
+    <td>ring</td>
+    <td>middle</td>
+    <td>index</td>
+    <td>thumb-bend</td>
+    <td>thumb-rotation</td>
+  </tr>
+</table>
+</div>

doc/灵巧手开发.md

@@ -0,0 +1,80 @@
+## 灵巧手介绍
+
+H1可搭载[Inspire Robotics](https://inspire-robots.com/product/frwz/)的仿人五指灵巧手，该灵巧手具有6个自由度和12个运动关节，可以模拟人手实现复杂动作。
+![RH56](./img/RH56.png)
+
+## 接口说明
+
+**示例程序** [h1_example_hand.cpp]()
+
+用户向 **"rt/inspire/cmd"** 话题发送 **"unitree_go::msg::dds::MotorCmds_"** 消息控制灵巧手。
+从 **"rt/inspire/state"** 话题接受 **"unitree_go::msg::dds::MotorStates_"** 消息获取灵巧手状态。
+
+```mermaid
+graph LR
+A(user) --rt/inspire/cmd--> B(H1)
+B --rt/inspire/state--> A
+```
+
++ IDL数据格式
+采用数组格式的电机数据，内部包含双手12个电机数据。
+```note
+当前灵巧手只支持关节控制，即在idl格式中只有参数q有意义。其他保留。
+```
+
+```bash
+# namespace unitree_go::msg::dds_
+
+# unitree_go::msg::dds_::MotorCmds_
+struct MotorCmds_ 
+{
+    sequence<unitree_go::msg::dds_::MotorCmd_> cmds;
+};
+
+# unitree_go::msg::dds_::MotorStates_
+struct MotorCmds_ 
+{
+    sequence<unitree_go::msg::dds_::MotorState> states;
+};
+```
+
+
++ IDL中的关节顺序
+<div style="text-align: center;">
+<table border="1">
+  <tr>
+    <td>Id</td>
+    <td>0</td>
+    <td>1</td>
+    <td>2</td>
+    <td>3</td>
+    <td>4</td>
+    <td>5</td>
+    <td>6</td>
+    <td>7</td>
+    <td>8</td>
+    <td>9</td>
+    <td>10</td>
+    <td>11</td>
+  </tr>
+  <tr>
+    <td rowspan="2">Joint</td>
+    <td colspan="6">Right Hand</td>
+    <td colspan="6">Left Hand</td>
+  </tr>
+  <tr>
+    <td>pinky</td>
+    <td>ring</td>
+    <td>middle</td>
+    <td>index</td>
+    <td>thumb-bend</td>
+    <td>thumb-rotation</td>
+    <td>pinky</td>
+    <td>ring</td>
+    <td>middle</td>
+    <td>index</td>
+    <td>thumb-bend</td>
+    <td>thumb-rotation</td>
+  </tr>
+</table>
+</div>

example/inspire_hand.cpp

@@ -0,0 +1,163 @@
+/**
+ * @file inspire_hand.cpp
+ * @brief This is an example of how to control the Unitree H1 (Inspire) Hand using unitree_sdk2.
+ */
+
+// Inspire Hand Topic IDL Types
+#include <unitree/idl/go2/MotorCmds_.hpp>
+#include <unitree/idl/go2/MotorStates_.hpp>
+// DDS Channel
+#include <unitree/robot/channel/channel_publisher.hpp>
+#include <unitree/robot/channel/channel_subscriber.hpp>
+#include <unitree/common/thread/thread.hpp>
+
+#include <eigen3/Eigen/Dense>
+#include <unordered_map>
+
+/**
+ * @brief Unitree H1 Hand Controller
+ * The user can subscribe to "rt/inspire/state" to get the current state of the hand and publish to "rt/inspire/cmd" to control the hand.
+ * 
+ *                  IDL Types
+ * user ---(unitree_go::msg::dds_::MotorCmds_)---> "rt/inspire/cmd"
+ * user <--(unitree_go::msg::dds_::MotorStates_)-- "rt/inspire/state"
+ * 
+ * @attention Currently the hand only supports position control, which means only the `q` field in idl is used.
+ */
+class H1HandController
+{
+public:
+    H1HandController()
+    {
+        this->InitDDS_();
+    }
+
+    /**
+     * @brief Control the hand to a specific label
+     */
+    void ctrl(std::string label)
+    {
+        if(labels.find(label) != labels.end())
+        {
+            this->ctrl(labels[label], labels[label]);
+        }
+        else
+        {
+            std::cout << "Invalid label: " << label << std::endl;
+        }
+    }
+
+    /**
+     * @brief Move the fingers to the specified angles
+     * 
+     * @note The angles should be in the range [0, 1]
+     *       0: close  1: open
+     */
+    void ctrl(
+        const Eigen::Matrix<float, 6, 1>& right_angles, 
+        const Eigen::Matrix<float, 6, 1>& left_angles)
+    {
+        for(size_t i(0); i<6; i++)
+        {
+            cmd.cmds()[i].q() = right_angles(i);
+            cmd.cmds()[i+6].q() = left_angles(i);
+        }
+        handcmd->Write(cmd);
+    }
+
+    /**
+     * @brief Get the right hand angles
+     * 
+     * Joint order: [pinky, ring, middle, index, thumb_bend, thumb_rotation]
+     */
+    Eigen::Matrix<float, 6, 1> getRightQ()
+    {
+        std::lock_guard<std::mutex> lock(mtx);
+        Eigen::Matrix<float, 6, 1> q;
+        for(size_t i(0); i<6; i++)
+        {
+            q(i) = state.states()[i].q();
+        }
+        return q;
+    }
+
+    /**
+     * @brief Get the left hand angles
+     * 
+     * Joint order: [pinky, ring, middle, index, thumb_bend, thumb_rotation]
+     */
+    Eigen::Matrix<float, 6, 1> getLeftQ()
+    {
+        std::lock_guard<std::mutex> lock(mtx);
+        Eigen::Matrix<float, 6, 1> q;
+        for(size_t i(0); i<6; i++)
+        {
+            q(i) = state.states()[i+6].q();
+        }
+        return q;
+    }
+
+    unitree_go::msg::dds_::MotorCmds_ cmd;
+    unitree_go::msg::dds_::MotorStates_ state;
+private:
+    void InitDDS_()
+    {
+        handcmd = std::make_shared<unitree::robot::ChannelPublisher<unitree_go::msg::dds_::MotorCmds_>>(
+            "rt/inspire/cmd");
+        handcmd->InitChannel();
+        cmd.cmds().resize(12);
+        handstate = std::make_shared<unitree::robot::ChannelSubscriber<unitree_go::msg::dds_::MotorStates_>>(
+            "rt/inspire/state");
+        handstate->InitChannel([this](const void *message){
+            std::lock_guard<std::mutex> lock(mtx);
+            state = *(unitree_go::msg::dds_::MotorStates_*)message;
+        });
+        state.states().resize(12);
+    }
+
+    // DDS parameters
+    std::mutex mtx;
+    unitree::robot::ChannelPublisherPtr<unitree_go::msg::dds_::MotorCmds_> handcmd;
+    unitree::robot::ChannelSubscriberPtr<unitree_go::msg::dds_::MotorStates_> handstate;
+
+    // Saved labels
+    std::unordered_map<std::string, Eigen::Matrix<float, 6, 1>> labels = {
+        {"open",   Eigen::Matrix<float, 6, 1>::Ones()},
+        {"close",  Eigen::Matrix<float, 6, 1>::Zero()},
+        {"half",   Eigen::Matrix<float, 6, 1>::Constant(0.5)},
+    };
+};
+
+/**
+ * Main Function
+ */
+int main(int argc, char** argv)
+{
+    std::cout << " --- Unitree Robotics --- \n";
+    std::cout << "     H1 Hand Example      \n\n";
+
+    // Target label
+    std::string label = "close"; // You change this value to other labels
+
+    // Initialize the DDS Channel
+    std::string networkInterface = argc > 1 ? argv[1] : "";
+    unitree::robot::ChannelFactory::Instance()->Init(0, networkInterface);
+
+    // Create the H1 Hand Controller
+    auto h1hand = std::make_shared<H1HandController>();
+
+    int cnt = 0;
+    while (true)
+    {
+        usleep(100000);
+        if(cnt++ % 10 == 0)
+            label = label == "close" ? "open" : "close";
+        h1hand->ctrl(label); // Control the hand
+        std::cout << "-- Hand State --\n";
+        std::cout << " R: " << h1hand->getRightQ().transpose() << std::endl;
+        std::cout << " L: " << h1hand->getLeftQ().transpose() << std::endl;
+        std::cout << "\033[3A"; // Move cursor up 3 lines
+    }
+
+    return 0;
+}

include/SerialPort.h

@@ -0,0 +1,154 @@
+#ifndef SERIAL_PORT_H
+#define SERIAL_PORT_H
+
+#include <termios.h>
+#include <sys/select.h>
+#include <string>
+#include <string.h>
+#include <fcntl.h>
+#include <sys/ioctl.h>
+#include <linux/serial.h>
+#include <unistd.h>
+#include <iostream>
+#include <memory>
+#include <chrono>
+#include <queue>
+
+void print_data(const uint8_t* data, uint8_t len)
+{
+  for (int i = 0; i < len; i++)
+  {
+    printf("%02x ", data[i]);
+  }
+  printf("\n");
+}
+
+class SerialPort
+{
+public:
+  using SharedPtr = std::shared_ptr<SerialPort>;
+
+  SerialPort(std::string port, speed_t baudrate, int timeout_ms = 2)
+  {
+    set_timeout(timeout_ms);
+    Init(port, baudrate);
+  }
+
+  ~SerialPort()
+  {
+    close(fd_);
+  }
+
+  ssize_t send(const uint8_t* data, size_t len)
+  {
+    // tcflush(fd_, TCIFLUSH);
+    ssize_t ret = ::write(fd_, data, len);
+    // tcdrain(fd_);
+    return ret;
+  }
+
+  ssize_t recv(uint8_t* data, size_t len)
+  {
+    FD_ZERO(&rSet_);
+    FD_SET(fd_, &rSet_);
+    ssize_t recv_len = 0;
+
+    switch (select(fd_ + 1, &rSet_, NULL, NULL, &timeout_))
+    {
+    case -1: // error
+      // std::cout << "communication error" << std::endl;
+      break;
+    case 0: // timeout
+      // std::cout << "timeout" << std::endl;
+      break;
+    default:
+      recv_len = ::read(fd_, data, len);
+      break;
+    }
+
+    return recv_len;
+  }
+
+  void recv(uint8_t* data, uint8_t head, ssize_t len)
+  {
+    // 存入队列
+    ssize_t recv_len = this->recv(recv_buf.data(), len);
+    for (int i = 0; i < recv_len; i++)
+    {
+      recv_queue.push(recv_buf[i]);
+    }
+
+    // 查找帧头
+    while (recv_queue.size() >= len)
+    {
+      if(recv_queue.front() != head)
+      {
+        recv_queue.pop();
+        continue;
+      }
+      break;
+    }
+
+    if(recv_queue.size() < len) return;
+
+    // 读取数据
+    for(int i = 0; i < len; i++)
+    {
+      data[i] = recv_queue.front();
+      recv_queue.pop();
+    }
+  }
+
+  void set_timeout(int timeout_ms)
+  {
+    timeout_.tv_sec = timeout_ms / 1000;
+    timeout_.tv_usec = (timeout_ms % 1000) * 1000;
+  }
+
+private:
+  void Init(std::string port, speed_t baudrate)
+  {
+    int ret;
+    // Open serial port
+    fd_ = open(port.c_str(), O_RDWR | O_NOCTTY);
+    if (fd_ < 0)
+    {
+      printf("Open serial port %s failed\n", port.c_str());
+      exit(-1);
+    }
+
+    // Set attributes
+    struct termios option;
+    memset(&option, 0, sizeof(option));
+    ret = tcgetattr(fd_, &option);
+
+    option.c_oflag = 0;
+    option.c_lflag = 0;
+    option.c_iflag = 0;
+
+    cfsetispeed(&option, baudrate);
+    cfsetospeed(&option, baudrate);
+
+    option.c_cflag &= ~CSIZE;
+    option.c_cflag |= CS8; // 8
+    option.c_cflag &= ~PARENB; // no parity
+    option.c_iflag &= ~INPCK; // no parity
+    option.c_cflag &= ~CSTOPB; // 1 stop bit
+
+    option.c_cc[VTIME] = 0;
+    option.c_cc[VMIN] = 0;
+    option.c_lflag |= CBAUDEX;
+
+    ret = tcflush(fd_, TCIFLUSH);
+    ret = tcsetattr(fd_, TCSANOW, &option);
+  }
+
+  int fd_;
+	fd_set rSet_;
+  timeval timeout_;
+
+  std::queue<uint8_t> recv_queue;
+  std::array<uint8_t, 1024> recv_buf;
+};
+
+#endif // SERIAL_PORT_H

include/inspire.h

@@ -0,0 +1,262 @@
+#ifndef INSPIRE_H
+#define INSPIRE_H
+
+#include <eigen3/Eigen/Dense>
+#include <vector>
+
+#include "SerialPort.h"
+
+namespace inspire
+{
+
+class InspireHand
+{
+public:
+  InspireHand(SerialPort::SharedPtr serial = nullptr, id_t id = 0)
+  : serial_(serial), id(id)
+  {
+    if(!serial)
+      serial_ = std::make_shared<SerialPort>("/dev/ttyUSB0", B115200);
+
+  }
+
+  void ChangeID(uint8_t before, uint8_t now)
+  {
+    std::vector<uint8_t> cmd = {0xEB, 0x90, before, 0x04, 0x12, 0xE8, 0x03, now, 0x00};
+    cmd.back() = CheckSum(cmd.data(), cmd.size());
+    serial_->send(cmd.data(), cmd.size());
+
+    usleep(5000);
+    serial_->recv(recvBuff, 9);
+  }
+
+
+  /**
+   * @brief 设置各自由度的位置
+   * -1: 不变
+   * [0, 1] 0: 闭合
+   * 小拇指、无名指、中指、食指、大拇指弯曲、大拇指旋转
+   */
+  int16_t SetPosition(const Eigen::Matrix<double, 6, 1> & q)
+  {
+    // 将q限制在0到1
+    Eigen::Matrix<int16_t, 6, 1> q_int16 = (q * 1000).cast<int16_t>().cwiseMax(0).cwiseMin(1000);
+
+
+    uint8_t cmd[20];
+    cmd[0] = 0xEB;    // head
+    cmd[1] = 0x90;
+    cmd[2] = id;      // ID
+    cmd[3] = 0x0F;    // data length
+    cmd[4] = 0x12;    // write
+    cmd[5] = 0xCE;    // address
+    cmd[6] = 0x05;
+
+    cmd[7] = q_int16(0) & 0xFF;
+    cmd[8] = (q_int16(0) >> 8) & 0xFF;
+    cmd[9] = q_int16(1) & 0xFF;
+    cmd[10] = (q_int16(1) >> 8) & 0xFF;
+    cmd[11] = q_int16(2) & 0xFF;
+    cmd[12] = (q_int16(2) >> 8) & 0xFF;
+    cmd[13] = q_int16(3) & 0xFF;
+    cmd[14] = (q_int16(3) >> 8) & 0xFF;
+    cmd[15] = q_int16(4) & 0xFF;
+    cmd[16] = (q_int16(4) >> 8) & 0xFF;
+    cmd[17] = q_int16(5) & 0xFF;
+    cmd[18] = (q_int16(5) >> 8) & 0xFF;
+
+    cmd[19] = CheckSum(cmd, 20);
+
+    serial_->send(cmd, 20);
+    usleep(5000);
+    serial_->recv(recvBuff, 9);
+    return 0;
+  }
+
+  /**
+   * @brief 读取当前各自由度的角度位置
+   * 
+   * 小拇指、无名指、中指、食指、大拇指弯曲、大拇指旋转
+   */
+  int16_t GetPosition(Eigen::Matrix<double, 6, 1> & q)
+  {
+    std::vector<uint8_t> cmd = {0xEB, 0x90, id, 0x04, 0x11, 0x0A, 0x06, 0x0C, 0x00};
+    cmd.back() = CheckSum(cmd.data(), cmd.size());
+    serial_->send(cmd.data(), cmd.size());
+
+    usleep(5000);
+    size_t len = serial_->recv(recvBuff, 20);
+
+    if(len != 20) return 1;
+    if(recvBuff[19] != CheckSum(recvBuff, 20)) return 2;
+
+    q(0) = (recvBuff[7] | (recvBuff[8] << 8)) / 1000.;
+    q(1) = (recvBuff[9] | (recvBuff[10] << 8)) / 1000.;
+    q(2) = (recvBuff[11] | (recvBuff[12] << 8)) / 1000.;
+    q(3) = (recvBuff[13] | (recvBuff[14] << 8)) / 1000.;
+    q(4) = (recvBuff[15] | (recvBuff[16] << 8)) / 1000.;
+    q(5) = (recvBuff[17] | (recvBuff[18] << 8)) / 1000.;
+
+    return 0;
+  }
+
+  /**
+   * @brief 设置各自由度的速度
+   * 
+   * 设置完速度后会立即生效，断电再上电不保存。
+   */
+  void SetVelocity(int16_t v0, int16_t v1, int16_t v2, int16_t v3, int16_t v4, int16_t v5)
+  {
+    uint8_t cmd[20];
+    cmd[0] = 0xEB;    // head
+    cmd[1] = 0x90;
+    cmd[2] = id;      // ID
+    cmd[3] = 0x0F;    // data length
+    cmd[4] = 0x12;    // write
+    cmd[5] = 0xF2;    // address
+    cmd[6] = 0x05;
+
+    cmd[7] = v0 & 0xFF;
+    cmd[8] = (v0 >> 8) & 0xFF;
+    cmd[9] = v1 & 0xFF;
+    cmd[10] = (v1 >> 8) & 0xFF;
+    cmd[11] = v2 & 0xFF;
+    cmd[12] = (v2 >> 8) & 0xFF;
+    cmd[13] = v3 & 0xFF;
+    cmd[14] = (v3 >> 8) & 0xFF;
+    cmd[15] = v4 & 0xFF;
+    cmd[16] = (v4 >> 8) & 0xFF;
+    cmd[17] = v5 & 0xFF;
+    cmd[18] = (v5 >> 8) & 0xFF;
+
+    cmd[19] = CheckSum(cmd, 20);
+
+    serial_->send(cmd, 20);
+    usleep(5000);
+    serial_->recv(recvBuff, 9);
+  }
+
+  /**
+   * @brief 各自由度的力控阈值设置值
+   * 
+   * [0, 1000] Unit: g
+   */
+  void SetForce(uint16_t f0, uint16_t f1, uint16_t f2, uint16_t f3, uint16_t f4, uint16_t f5)
+  {
+    uint8_t cmd[20];
+    cmd[0] = 0xEB;    // head
+    cmd[1] = 0x90;
+    cmd[2] = id;      // ID
+    cmd[3] = 0x0F;    // data length
+    cmd[4] = 0x12;    // write
+    cmd[5] = 0xDA;    // address
+    cmd[6] = 0x05;
+
+    cmd[7] = f0 & 0xFF;
+    cmd[8] = (f0 >> 8) & 0xFF;
+    cmd[9] = f1 & 0xFF;
+    cmd[10] = (f1 >> 8) & 0xFF;
+    cmd[11] = f2 & 0xFF;
+    cmd[12] = (f2 >> 8) & 0xFF;
+    cmd[13] = f3 & 0xFF;
+    cmd[14] = (f3 >> 8) & 0xFF;
+    cmd[15] = f4 & 0xFF;
+    cmd[16] = (f4 >> 8) & 0xFF;
+    cmd[17] = f5 & 0xFF;
+    cmd[18] = (f5 >> 8) & 0xFF;
+
+    cmd[19] = CheckSum(cmd, 20);
+
+    serial_->send(cmd, 20);
+    usleep(5000);
+    serial_->recv(recvBuff, 9);
+  }
+
+  /**
+   * @brief 各手指的实际受力
+   * 
+   * 原本单位为 g, [0 - 1000], 转为 N
+   */
+  int16_t GetForce(Eigen::Matrix<double, 6, 1> & f)
+  {
+    std::vector<uint8_t> cmd = {0xEB, 0x90, id, 0x04, 0x11, 0x2E, 0x06, 0x0C, 0x00};
+    cmd.back() = CheckSum(cmd.data(), cmd.size());
+    serial_->send(cmd.data(), cmd.size());
+
+    usleep(5000);
+    size_t len = serial_->recv(recvBuff, 20);
+
+    if(len != 20) return 1;
+    if(recvBuff[19] != CheckSum(recvBuff, 20)) return 2;
+
+    f(0) = int16_t(recvBuff[7] | (recvBuff[8] << 8)) / 1000. * 9.8;
+    f(1) = int16_t(recvBuff[9] | (recvBuff[10] << 8)) / 1000. * 9.8;
+    f(2) = int16_t(recvBuff[11] | (recvBuff[12] << 8)) / 1000. * 9.8;
+    f(3) = int16_t(recvBuff[13] | (recvBuff[14] << 8)) / 1000. * 9.8;
+    f(4) = int16_t(recvBuff[15] | (recvBuff[16] << 8)) / 1000. * 9.8;
+    f(5) = int16_t(recvBuff[17] | (recvBuff[18] << 8)) / 1000. * 9.8;
+
+    return 0;
+  }
+
+  /**
+   * @brief 清除错误
+   * 
+   * 当灵巧手发生堵转，过流，异常等故障可通过清除故障指令恢复正常运行
+   */
+  void ClearError()
+  {
+    uint8_t cmd[9];
+    cmd[0] = 0xEB;    // head
+    cmd[1] = 0x90;
+    cmd[2] = id;      // ID
+    cmd[3] = 0x04;    // data length
+    cmd[4] = 0x12;    // write
+    cmd[5] = 0xEC;    // address
+    cmd[6] = 0x03;
+    cmd[7] = 0x01;
+    cmd[8] = CheckSum(cmd, 9);
+    
+    serial_->send(cmd, 9);
+    usleep(5000);
+    serial_->recv(recvBuff, 9);
+  }
+
+  /**
+   * @brief 受力传感器校准
+   * 
+   * 校准时需保证灵巧手处于空载状态
+   */
+  void Calibration()
+  {
+    std::vector<uint8_t> cmd = {0xEB, 0x90, id, 0x04, 0x12, 0x2F, 0x06, 0x01, 0x00};
+    cmd.back() = CheckSum(cmd.data(), cmd.size());
+    serial_->send(cmd.data(), cmd.size());
+
+    usleep(5000);
+    serial_->recv(recvBuff, 9); // 第一帧
+    sleep(10); // 整个过程大约需要6s
+    serial_->recv(recvBuff, 9); // 第二帧
+    std::cout << "Calibration: ";
+    print_data(recvBuff, 9);
+  }
+
+  uint8_t id = 1;
+private:
+  uint8_t CheckSum(const uint8_t* data, uint8_t len)
+  {
+    uint8_t sum = 0;
+    // 除应答帧头外其余数据的累加和的低字节
+    for (int i = 2; i < len - 1; i++)
+    {
+      sum += data[i];
+    }
+    return sum;
+  }
+
+  SerialPort::SharedPtr serial_;
+  uint8_t recvBuff[1024];
+};
+
+} // namespace inspire
+#endif // INSPIRE_H

include/param.h

@@ -0,0 +1,58 @@
+#ifndef PARAM_H
+#define PARAM_H
+
+#include <stdint.h>
+#include <iostream>
+#include <chrono>
+#include <spdlog/spdlog.h>
+#include <boost/program_options.hpp>
+
+namespace param
+{
+
+namespace po = boost::program_options;
+
+inline std::string serial_port;
+inline std::string network; 
+inline std::string ns; 
+inline float threhold;
+
+po::variables_map helper(int argc, char** argv)
+{
+#ifndef NDEBUG
+  spdlog::set_level(spdlog::level::debug);
+#else
+  spdlog::set_level(spdlog::level::info);
+#endif
+
+
+  po::options_description desc("Unitree H1 Inspire Hand Serial to DDS");
+  desc.add_options()
+    ("help,h", "produce help message")
+    ("serial,s", po::value<std::string>(&serial_port)->default_value("/dev/h1_newhand"), "serial port")
+    ("network", po::value<std::string>(&network)->default_value(""), "DDS network interface")
+    ("namespace", po::value<std::string>(&ns)->default_value("inspire"), "DDS topic namespace")
+    ;
+
+  po::variables_map vm;
+  po::store(po::parse_command_line(argc, argv, desc), vm);
+  po::notify(vm);
+
+  if (vm.count("help"))
+  {
+    std::cout << desc << std::endl;
+    exit(0);
+  }
+
+  if(ns.empty())
+  {
+    spdlog::error("Namespace cannot be empty");
+    exit(1);
+  }
+
+  return vm;
+}
+
+}
+
+#endif // PARAM_H

inspire_ctrl.cpp

@@ -0,0 +1,116 @@
+#include "inspire.h"
+#include <unitree/dds_wrapper/go2/go2_pub.h>
+#include <unitree/dds_wrapper/go2/go2_sub.h>
+#include "param.h"
+
+class InspireRunner
+{
+public:
+  InspireRunner()
+  {
+    serial = std::make_shared<SerialPort>(param::serial_port, B115200);
+
+    // inspire
+    righthand = std::make_shared<inspire::InspireHand>(serial, 1); // 按邵博格式，先右后左
+    lefthand = std::make_shared<inspire::InspireHand>(serial, 2);
+
+    // dds
+    handcmd = std::make_shared<unitree::robot::SubscriptionBase<unitree_go::msg::dds_::MotorCmds_>>(
+        "rt/" + param::ns + "/cmd");
+    handcmd->msg_.cmds().resize(12);
+    handstate = std::make_unique<unitree::robot::go2::publisher::RealTimeMotorStates>(
+        "rt/" + param::ns + "/state");
+    handstate->msg_.states().resize(12);
+
+    // Start running
+    thread = std::make_shared<unitree::common::RecurrentThread>(
+      10000, std::bind(&InspireRunner::run, this)
+    );
+  }
+
+  void run()
+  {
+    // Set command
+    if(!handcmd->isTimeout())
+    {
+      for(int i(0); i<12; i++)
+      {
+        qcmd(i) = handcmd->msg_.cmds()[i].q();
+      }
+      righthand->SetPosition(qcmd.block<6, 1>(0, 0)); // 先右后左
+      lefthand->SetPosition(qcmd.block<6, 1>(6, 0));
+    }
+
+    // Recv state
+    Eigen::Matrix<double, 6, 1> qtemp;
+    if(righthand->GetPosition(qtemp) == 0)
+    {
+      qstate.block<6, 1>(0, 0) = qtemp;
+    }
+    else
+    {
+      for(int i(0); i<6; i++)
+      {
+        handstate->msg_.states()[i].lost()++;
+      }
+      spdlog::debug("Failed to get right hand state");
+    }
+    if(lefthand->GetPosition(qtemp) == 0)
+    {
+      qstate.block<6, 1>(6, 0) = qtemp;
+    }
+    else
+    {
+      for(int i(0); i<6; i++)
+      {
+        handstate->msg_.states()[i+6].lost()++;
+      }
+      spdlog::debug("Failed to get left hand state");
+    }
+    if(handstate->trylock())
+    {
+        for(int i(0); i<12; i++)
+        {
+            handstate->msg_.states()[i].q() = qstate(i);
+        }
+        handstate->unlockAndPublish();
+    }
+  }
+
+  unitree::common::ThreadPtr thread;
+
+  // inspire
+  SerialPort::SharedPtr serial;
+  std::shared_ptr<inspire::InspireHand> lefthand;
+  std::shared_ptr<inspire::InspireHand> righthand;
+  Eigen::Matrix<double, 12, 1> qcmd, qstate;
+
+  // dds
+  std::shared_ptr<unitree::robot::SubscriptionBase<unitree_go::msg::dds_::MotorCmds_>> handcmd;
+  std::unique_ptr<unitree::robot::go2::publisher::RealTimeMotorStates> handstate;
+};
+
+int main(int argc, char ** argv)
+{
+  auto vm = param::helper(argc, argv);
+  if (param::network.empty())
+  {
+    // 在H1上需使用master service启动DDS，不带参数，查找程序同级目录下的parameter.json
+    // 否则会报错
+    unitree::robot::ChannelFactory::Instance()->Init();
+  }
+  else {
+    unitree::robot::ChannelFactory::Instance()->Init(0, param::network);
+  }
+
+  std::cout << " --- Unitree Robotics --- " << std::endl;
+  std::cout << "  Inspire Hand Controller  " << std::endl;
+
+  InspireRunner runner;
+ 
+  while (true)
+  {
+    sleep(1);
+  }
+  return 0;
+}