commit b52c204997969463f80d1354d1c1c49dcdee797b Author: Agnel Wang <2273421791wk@gmail.com> Date: Tue Apr 2 19:50:19 2024 +0800 initial release diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..5d7d730 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,3 @@ +{ + "ros.distro": "noetic" +} \ No newline at end of file diff --git a/CHANGELOG.md b/CHANGELOG.md new file mode 100644 index 0000000..ab468f8 --- /dev/null +++ b/CHANGELOG.md @@ -0,0 +1,5 @@ +# Changelog + +## 1.0.1 + ++ Initial release. \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..a07a05e --- /dev/null +++ b/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) \ No newline at end of file diff --git a/bin/x86_64/h1_hand_example b/bin/x86_64/h1_hand_example new file mode 100755 index 0000000..04e8ad1 Binary files /dev/null and b/bin/x86_64/h1_hand_example differ diff --git a/bin/x86_64/inspire_hand b/bin/x86_64/inspire_hand new file mode 100755 index 0000000..ce940b0 Binary files /dev/null and b/bin/x86_64/inspire_hand differ diff --git a/doc/Inspire-Hand.md b/doc/Inspire-Hand.md new file mode 100644 index 0000000..c5fe034 --- /dev/null +++ b/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 cmds; +}; + +# unitree_go::msg::dds_::MotorStates_ +struct MotorCmds_ +{ + sequence states; +}; +``` ++ Joint Motor Sequence + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Id01234567891011
JointRight HandLeft Hand
pinkyringmiddleindexthumb-bendthumb-rotationpinkyringmiddleindexthumb-bendthumb-rotation
+
\ No newline at end of file diff --git a/doc/img/RH56.png b/doc/img/RH56.png new file mode 100644 index 0000000..c8fc6ab Binary files /dev/null and b/doc/img/RH56.png differ diff --git a/doc/灵巧手开发.md b/doc/灵巧手开发.md new file mode 100644 index 0000000..97a2f65 --- /dev/null +++ b/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 cmds; +}; + +# unitree_go::msg::dds_::MotorStates_ +struct MotorCmds_ +{ + sequence states; +}; +``` + + ++ IDL中的关节顺序 +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Id01234567891011
JointRight HandLeft Hand
pinkyringmiddleindexthumb-bendthumb-rotationpinkyringmiddleindexthumb-bendthumb-rotation
+
\ No newline at end of file diff --git a/example/inspire_hand.cpp b/example/inspire_hand.cpp new file mode 100644 index 0000000..8abdf99 --- /dev/null +++ b/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 +#include +// DDS Channel +#include +#include +#include + +#include +#include + +/** + * @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& right_angles, + const Eigen::Matrix& 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 getRightQ() + { + std::lock_guard lock(mtx); + Eigen::Matrix 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 getLeftQ() + { + std::lock_guard lock(mtx); + Eigen::Matrix 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>( + "rt/inspire/cmd"); + handcmd->InitChannel(); + cmd.cmds().resize(12); + handstate = std::make_shared>( + "rt/inspire/state"); + handstate->InitChannel([this](const void *message){ + std::lock_guard lock(mtx); + state = *(unitree_go::msg::dds_::MotorStates_*)message; + }); + state.states().resize(12); + } + + // DDS parameters + std::mutex mtx; + unitree::robot::ChannelPublisherPtr handcmd; + unitree::robot::ChannelSubscriberPtr handstate; + + // Saved labels + std::unordered_map> labels = { + {"open", Eigen::Matrix::Ones()}, + {"close", Eigen::Matrix::Zero()}, + {"half", Eigen::Matrix::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(); + + 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; +} \ No newline at end of file diff --git a/include/SerialPort.h b/include/SerialPort.h new file mode 100644 index 0000000..e057dae --- /dev/null +++ b/include/SerialPort.h @@ -0,0 +1,154 @@ +#ifndef SERIAL_PORT_H +#define SERIAL_PORT_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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(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 recv_queue; + std::array recv_buf; +}; + +#endif // SERIAL_PORT_H \ No newline at end of file diff --git a/include/inspire.h b/include/inspire.h new file mode 100644 index 0000000..ffa1e19 --- /dev/null +++ b/include/inspire.h @@ -0,0 +1,262 @@ +#ifndef INSPIRE_H +#define INSPIRE_H + +#include +#include + +#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("/dev/ttyUSB0", B115200); + + } + + void ChangeID(uint8_t before, uint8_t now) + { + std::vector 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 & q) + { + // 将q限制在0到1 + Eigen::Matrix q_int16 = (q * 1000).cast().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 & q) + { + std::vector 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 & f) + { + std::vector 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 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 \ No newline at end of file diff --git a/include/param.h b/include/param.h new file mode 100644 index 0000000..b56a8d8 --- /dev/null +++ b/include/param.h @@ -0,0 +1,58 @@ +#ifndef PARAM_H +#define PARAM_H + +#include +#include +#include +#include +#include + +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(&serial_port)->default_value("/dev/h1_newhand"), "serial port") + ("network", po::value(&network)->default_value(""), "DDS network interface") + ("namespace", po::value(&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 \ No newline at end of file diff --git a/inspire_ctrl.cpp b/inspire_ctrl.cpp new file mode 100644 index 0000000..a94baf8 --- /dev/null +++ b/inspire_ctrl.cpp @@ -0,0 +1,116 @@ +#include "inspire.h" +#include +#include +#include "param.h" + +class InspireRunner +{ +public: + InspireRunner() + { + serial = std::make_shared(param::serial_port, B115200); + + // inspire + righthand = std::make_shared(serial, 1); // 按邵博格式，先右后左 + lefthand = std::make_shared(serial, 2); + + // dds + handcmd = std::make_shared>( + "rt/" + param::ns + "/cmd"); + handcmd->msg_.cmds().resize(12); + handstate = std::make_unique( + "rt/" + param::ns + "/state"); + handstate->msg_.states().resize(12); + + // Start running + thread = std::make_shared( + 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 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 lefthand; + std::shared_ptr righthand; + Eigen::Matrix qcmd, qstate; + + // dds + std::shared_ptr> handcmd; + std::unique_ptr 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; +} \ No newline at end of file