diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..378eac2 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +build diff --git a/.vscode/settings.json b/.vscode/settings.json deleted file mode 100644 index 5d7d730..0000000 --- a/.vscode/settings.json +++ /dev/null @@ -1,3 +0,0 @@ -{ - "ros.distro": "noetic" -} \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index a07a05e..307706a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,8 +1,8 @@ cmake_minimum_required(VERSION 3.10) -project(h1_inspire_service VERSION 0.1.0) +project(inspire_service) if(NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Debug) + set(CMAKE_BUILD_TYPE Release) endif() if (CMAKE_BUILD_TYPE MATCHES "Debug") @@ -13,6 +13,10 @@ else() set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17 -O3 -DNDEBUG -fPIC") endif() +if(POLICY CMP0167) + cmake_policy(SET CMP0167 NEW) +endif() + find_package(Boost COMPONENTS program_options REQUIRED) include_directories( @@ -22,9 +26,10 @@ include_directories( ) link_libraries(unitree_sdk2 ddsc ddscxx rt pthread) -link_libraries(unitree_idl libboost_program_options.a) +link_libraries(libboost_program_options.a) -add_executable(inspire_hand inspire_ctrl.cpp) +add_executable(inspire_g1 inspire_g1.cpp) +add_executable(inspire_h1 inspire_h1.cpp) # example -add_executable(h1_hand_example example/inspire_hand.cpp) \ No newline at end of file +add_executable(hand_example example/hand_example.cpp) \ No newline at end of file diff --git a/README.md b/README.md new file mode 100644 index 0000000..264010b --- /dev/null +++ b/README.md @@ -0,0 +1,121 @@ +
+

DFX Inspire Hand service

+ + Unitree LOGO + +
+ +# 1. 📦 Introduction + +[Unitree Robot RH56DFX Inspire Hand](https://support.unitree.com/home/en/H1_developer/Dexterous_hand) Controller. + +
+ +
+ + +The user sends `unitree_go::msg::dds::MotorCmds_` messages to the `rt/inspire/cmd` topic to control the dexterous hand, +and receives `unitree_go::msg::dds::MotorStates_` messages from the `rt/inspire/state` topic to obtain its status. + +```mermaid +graph LR +A(user) --rt/inspire/cmd--> B(H1) +B --rt/inspire/state--> A +``` + +The IDL data is an array containing joint-level values for all 12 motors of both hands. +Currently the dexterous hand only supports joint control, i.e. only the parameter `q` makes sense in the idl format. The others are reserved. + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Id01234567891011
JointRight HandLeft Hand
pinkyringmiddleindexthumb-bendthumb-rotationpinkyringmiddleindexthumb-bendthumb-rotation
+
+ + +# 2. 🚀 Launch + +## unitree h1 +```bash +# Build project +mkdir build & cd build +cmake .. +make -j6 +# Terminal 1. Run h1 inspire hand service +sudo ./inspire_h1 -s /dev/ttyUSB0 +# Terminal 2. Run example +./hand_example +``` + +## unitree g1 +```bash +# Build project +mkdir build & cd build +cmake .. +make -j6 +# Terminal 1. Run g1 inspire hand service +# The serial port name is hard-coded; if it doesn’t match your setup, please edit it directly in the source. +sudo ./inspire_g1 +# Terminal 2. Run example +./hand_example +``` + +# FAQ +1. Error when `make -j6` + ```bash + ... + /usr/bin/ld: inspire_ctrl.cpp:(.text._Z14serialize_intoIN10unitree_go3msg4dds_12MotorStates_EN3org7eclipse10cyclonedds4core3cdr14xcdr_v2_streamEEbPvmRKT_b[_Z14serialize_intoIN10unitree_go3msg4dds_12MotorStates_EN3org7eclipse10cyclonedds4core3cdr14xcdr_v2_streamEEbPvmRKT_b]+0x2be): undefined reference to `org::eclipse::cyclonedds::core::cdr::xcdr_v2_stream::finish_member(org::eclipse::cyclonedds::core::cdr::entity_properties&, bool)' + /usr/bin/ld: inspire_ctrl.cpp:(.text._Z14serialize_intoIN10unitree_go3msg4dds_12MotorStates_EN3org7eclipse10cyclonedds4core3cdr14xcdr_v2_streamEEbPvmRKT_b[_Z14serialize_intoIN10unitree_go3msg4dds_12MotorStates_EN3org7eclipse10cyclonedds4core3cdr14xcdr_v2_streamEEbPvmRKT_b]+0x2d9): undefined reference to `org::eclipse::cyclonedds::core::cdr::xcdr_v2_stream::finish_struct(org::eclipse::cyclonedds::core::cdr::entity_properties&)' + + ``` + please compile and install `unitree_sdk2`: + ```bash + cd ~ + git clone https://github.com/unitreerobotics/unitree_sdk2 + cd unitree_sdk2 + mkdir build & cd build + cmake .. + sudo make install + ``` +2. Error when run `sudo ./inspire_h1 -s /dev/ttyUSB0` or `sudo ./inspire_g1` + ```bash + --- Unitree Robotics --- + Inspire Hand Controller + Open serial port /dev/ttyUSB* failed + ``` + For **Unitree h1**, use the `-s` parameter to change the serial port name. + For **Unitree g1**, modify the serial port name directly in the source code. diff --git a/bin/x86_64/h1_hand_example b/bin/x86_64/h1_hand_example deleted file mode 100755 index 04e8ad1..0000000 Binary files a/bin/x86_64/h1_hand_example and /dev/null differ diff --git a/bin/x86_64/inspire_hand b/bin/x86_64/inspire_hand deleted file mode 100755 index ce940b0..0000000 Binary files a/bin/x86_64/inspire_hand and /dev/null differ diff --git a/doc/Inspire-Hand.md b/doc/Inspire-Hand.md deleted file mode 100644 index c5fe034..0000000 --- a/doc/Inspire-Hand.md +++ /dev/null @@ -1,80 +0,0 @@ -## 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/灵巧手开发.md b/doc/灵巧手开发.md deleted file mode 100644 index 97a2f65..0000000 --- a/doc/灵巧手开发.md +++ /dev/null @@ -1,80 +0,0 @@ -## 灵巧手介绍 - -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/hand_example.cpp similarity index 100% rename from example/inspire_hand.cpp rename to example/hand_example.cpp diff --git a/include/SerialPort.h b/include/SerialPort.h index e057dae..cf7704f 100644 --- a/include/SerialPort.h +++ b/include/SerialPort.h @@ -14,15 +14,6 @@ #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: @@ -41,9 +32,7 @@ public: ssize_t send(const uint8_t* data, size_t len) { - // tcflush(fd_, TCIFLUSH); ssize_t ret = ::write(fd_, data, len); - // tcdrain(fd_); return ret; } @@ -69,36 +58,6 @@ public: 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; diff --git a/include/dds/Publisher.h b/include/dds/Publisher.h new file mode 100644 index 0000000..97a8f37 --- /dev/null +++ b/include/dds/Publisher.h @@ -0,0 +1,172 @@ +#ifndef _UT_ROBOT_PUBLISHER_H_ +#define _UT_ROBOT_PUBLISHER_H_ + +#include +#include +#include +#include + +namespace unitree +{ +namespace robot +{ + +template +class PublisherBase : public unitree::robot::ChannelPublisher +{ +public: + using MsgType = MessageType; + using SharedPtr = std::shared_ptr>; + + PublisherBase(std::string TOPIC_NAME) + : unitree::robot::ChannelPublisher(TOPIC_NAME) + { + this->InitChannel(); + } +}; + +// For details: see https://github.com/ros-controls/realtime_tools +template +class RealTimePublisher +{ +public: + using MsgType = MessageType; + using PublisherSharedPtr = typename unitree::robot::ChannelPublisherPtr; + + MessageType msg_; + + explicit RealTimePublisher(PublisherSharedPtr publisher) + : publisher_(publisher), is_running_(false), keep_running_(true), turn_(LOOP_NOT_STARTED) + { + thread_ = std::thread(&RealTimePublisher::publishingLoop, this); + } + + explicit RealTimePublisher(std::string topic) + : RealTimePublisher(std::make_shared>(topic)) + {} + + ~RealTimePublisher() + { + stop(); + while (is_running()) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + if(thread_.joinable()) { thread_.join(); } + } + + void stop() + { + keep_running_ = false; + } + + /** + * @brief Try to get the data lock from realtime + * + * To publish data from the realtime loop, you need to run trylock to + * attenot to get unique access to the msg_ variable. Teylock returns + * true if the lock was aquired, and false otherwise. + */ + bool trylock() + { + if(mutex_.try_lock()) + { + if(turn_ == REALTIME) { + return true; + } else { + mutex_.unlock(); + return false; + } + } else { + return false; + } + } + + /** + * @brief Unlock the msg_ variable and publish it. + */ + void unlockAndPublish() + { + turn_ = NON_REALTIME; + mutex_.unlock(); + } + + /** + * @brief Get the data lock from non-realtime. + * + * To publish data from the realtime loop, you need to run trylock to + * attenot to get unique access to the msg_ variable. Teylock returns + * true if the lock was aquired, and false otherwise. + */ + void lock() + { + // never actually lock on the lock + while (!mutex_.try_lock()) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + } + + /** + * @brief Unlocks the data without publishing anything. + */ + void unlock() { mutex_.unlock(); } + +protected: + /** + * @brief Something before sending the message. + */ + virtual void post_communication() {} + +private: + // non-copyable + RealTimePublisher(const RealTimePublisher&) = delete; + RealTimePublisher& operator=(const RealTimePublisher&) = delete; + + + bool is_running() const { return is_running_; } + + void publishingLoop() + { + is_running_ = true; + turn_ = REALTIME; + + while (keep_running_) + { + MsgType outgoing; + + // Locks msg_ and copies it + lock(); + while (turn_ != NON_REALTIME && keep_running_) + { + unlock(); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + lock(); + } + post_communication(); + outgoing = msg_; + turn_ = REALTIME; + + unlock(); + + if(keep_running_) { + publisher_->Write(outgoing, 0); + } + } + is_running_ = false; + } + + PublisherSharedPtr publisher_; + std::atomic_bool is_running_; + std::atomic_bool keep_running_; + + std::mutex mutex_; + + std::thread thread_; + + enum { REALTIME, NON_REALTIME, LOOP_NOT_STARTED }; + std::atomic turn_; +}; + +} // namespace robot +} // namespace unitree + +#endif // _UT_ROBOT_PUBLISHER_H_ \ No newline at end of file diff --git a/include/dds/Subscription.h b/include/dds/Subscription.h new file mode 100644 index 0000000..be9d8f2 --- /dev/null +++ b/include/dds/Subscription.h @@ -0,0 +1,54 @@ +#ifndef _UT_ROBOT_SUBSCRIPTION_H_ +#define _UT_ROBOT_SUBSCRIPTION_H_ + +#include +#include + +namespace unitree +{ +namespace robot +{ + +template +class SubscriptionBase +{ +public: + using MsgType = MessageType; + using SharedPtr = std::shared_ptr>; + + SubscriptionBase(const std::string& topic) + { + last_update_time_ = std::chrono::steady_clock::now() - std::chrono::milliseconds(timeout_ms_); + sub_ = std::make_shared>(topic); + sub_->InitChannel([this](const void *msg){ + last_update_time_ = std::chrono::steady_clock::now(); + std::lock_guard lock(mutex_); + msg_ = *(const MessageType*)msg; + post_communication(); + }); + } + + void set_timeout_ms(uint32_t timeout_ms) { timeout_ms_ = timeout_ms; } + + bool isTimeout() { + auto now = std::chrono::steady_clock::now(); + auto elasped_time = now - last_update_time_; + return elasped_time > std::chrono::milliseconds(timeout_ms_); + } + + + MessageType msg_; +protected: + virtual void post_communication() {} // something after receiving message + + uint32_t timeout_ms_{1000}; + std::mutex mutex_; + unitree::robot::ChannelSubscriberPtr sub_; + std::chrono::steady_clock::time_point last_update_time_; +}; + + +}; // namespace robot +}; // namespace unitree + +#endif // _UT_ROBOT_SUBSCRIPTION_H_ \ No newline at end of file diff --git a/include/inspire.h b/include/inspire.h index ffa1e19..bc40f12 100644 --- a/include/inspire.h +++ b/include/inspire.h @@ -32,17 +32,15 @@ public: /** - * @brief 设置各自由度的位置 - * -1: 不变 - * [0, 1] 0: 闭合 - * 小拇指、无名指、中指、食指、大拇指弯曲、大拇指旋转 + * @brief Set the finger position + * -1: no change + * [0, 1] 0: close + * ID: pinky, ring, middle, index, thumb_bend, thumb_rotate */ 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; @@ -74,9 +72,9 @@ public: } /** - * @brief 读取当前各自由度的角度位置 + * @brief Get the finger position * - * 小拇指、无名指、中指、食指、大拇指弯曲、大拇指旋转 + * ID: pinky, ring, middle, index, thumb_bend, thumb_rotate */ int16_t GetPosition(Eigen::Matrix & q) { @@ -101,9 +99,7 @@ public: } /** - * @brief 设置各自由度的速度 - * - * 设置完速度后会立即生效，断电再上电不保存。 + * @brief Set the finger velocity */ void SetVelocity(int16_t v0, int16_t v1, int16_t v2, int16_t v3, int16_t v4, int16_t v5) { @@ -137,7 +133,7 @@ public: } /** - * @brief 各自由度的力控阈值设置值 + * @brief Get the force control threshold * * [0, 1000] Unit: g */ @@ -173,9 +169,9 @@ public: } /** - * @brief 各手指的实际受力 + * @brief Get the force of each finger * - * 原本单位为 g, [0 - 1000], 转为 N + * The force is in the unit of g, [0 - 1000], convert to N */ int16_t GetForce(Eigen::Matrix & f) { @@ -200,9 +196,10 @@ public: } /** - * @brief 清除错误 + * @brief Clear error * - * 当灵巧手发生堵转，过流，异常等故障可通过清除故障指令恢复正常运行 + * When the Inspire Hand has a fault such as a stall, overcurrent, or abnormality, + * the fault can be cleared by the clear fault command. */ void ClearError() { @@ -223,9 +220,9 @@ public: } /** - * @brief 受力传感器校准 + * @brief Force sensor calibration * - * 校准时需保证灵巧手处于空载状态 + * @attention The Inspire Hand must be in an unloaded state during calibration */ void Calibration() { @@ -234,11 +231,9 @@ public: 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); + serial_->recv(recvBuff, 9); // First frame + sleep(10); // The calibration process takes about 6 seconds + serial_->recv(recvBuff, 9); // Second frame } uint8_t id = 1; @@ -246,7 +241,6 @@ 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]; diff --git a/include/param.h b/include/param.h index b56a8d8..496eab8 100644 --- a/include/param.h +++ b/include/param.h @@ -29,7 +29,7 @@ po::variables_map helper(int argc, char** argv) 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") + ("serial,s", po::value(&serial_port)->default_value("/dev/ttyUSB0"), "serial port") ("network", po::value(&network)->default_value(""), "DDS network interface") ("namespace", po::value(&ns)->default_value("inspire"), "DDS topic namespace") ; diff --git a/inspire_g1.cpp b/inspire_g1.cpp new file mode 100644 index 0000000..44454c5 --- /dev/null +++ b/inspire_g1.cpp @@ -0,0 +1,114 @@ +#include "inspire.h" +#include "param.h" + +#include "dds/Publisher.h" +#include "dds/Subscription.h" +#include +#include +#include + +class InspireRunner +{ +public: + InspireRunner() + { + serial1 = std::make_shared("/dev/ttyUSB1", B115200); + serial2 = std::make_shared("/dev/ttyUSB2", B115200); + + // If your left and right hand controls are reversed, you can swap the positions of `serial1` and `serial2` below. + righthand = std::make_shared(serial1, 1); + lefthand = std::make_shared(serial2, 1); + + // 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 serial1; + SerialPort::SharedPtr serial2; + std::shared_ptr lefthand; + std::shared_ptr righthand; + Eigen::Matrix qcmd, qstate; + + // dds + std::unique_ptr> handstate; + std::shared_ptr> handcmd; +}; + +int main(int argc, char ** argv) +{ + auto vm = param::helper(argc, argv); + 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 diff --git a/inspire_ctrl.cpp b/inspire_h1.cpp similarity index 73% rename from inspire_ctrl.cpp rename to inspire_h1.cpp index a94baf8..ffce738 100644 --- a/inspire_ctrl.cpp +++ b/inspire_h1.cpp @@ -1,8 +1,12 @@ #include "inspire.h" -#include -#include #include "param.h" +#include "dds/Publisher.h" +#include "dds/Subscription.h" +#include +#include +#include + class InspireRunner { public: @@ -11,14 +15,14 @@ public: serial = std::make_shared(param::serial_port, B115200); // inspire - righthand = std::make_shared(serial, 1); // 按邵博格式，先右后左 - lefthand = std::make_shared(serial, 2); + righthand = std::make_shared(serial, 1); // ID 1 + lefthand = std::make_shared(serial, 2); // ID 2 // dds handcmd = std::make_shared>( "rt/" + param::ns + "/cmd"); handcmd->msg_.cmds().resize(12); - handstate = std::make_unique( + handstate = std::make_unique>( "rt/" + param::ns + "/state"); handstate->msg_.states().resize(12); @@ -37,7 +41,7 @@ public: { qcmd(i) = handcmd->msg_.cmds()[i].q(); } - righthand->SetPosition(qcmd.block<6, 1>(0, 0)); // 先右后左 + righthand->SetPosition(qcmd.block<6, 1>(0, 0)); lefthand->SetPosition(qcmd.block<6, 1>(6, 0)); } @@ -53,7 +57,7 @@ public: { handstate->msg_.states()[i].lost()++; } - spdlog::debug("Failed to get right hand state"); + // spdlog::debug("Failed to get right hand state"); } if(lefthand->GetPosition(qtemp) == 0) { @@ -65,7 +69,7 @@ public: { handstate->msg_.states()[i+6].lost()++; } - spdlog::debug("Failed to get left hand state"); + // spdlog::debug("Failed to get left hand state"); } if(handstate->trylock()) { @@ -86,22 +90,14 @@ public: Eigen::Matrix qcmd, qstate; // dds + std::unique_ptr> handstate; 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); - } + unitree::robot::ChannelFactory::Instance()->Init(0, param::network); std::cout << " --- Unitree Robotics --- " << std::endl; std::cout << " Inspire Hand Controller " << std::endl;