kassam/DFX_inspire_service
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

[update] support g1.

Browse Source
This commit is contained in:
silencht 2025-08-18 10:56:30 +08:00
parent b52c204997
commit 74d0305a2a
16 changed files with 505 additions and 252 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored Normal file
View File

@ -0,0 +1 @@
build

3
.vscode/settings.json vendored
View File

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

15
CMakeLists.txt
View File

@ -1,8 +1,8 @@
cmake_minimum_required(VERSION 3.10) cmake_minimum_required(VERSION 3.10)
project(h1_inspire_service VERSION 0.1.0) project(inspire_service)
if(NOT CMAKE_BUILD_TYPE) if(NOT CMAKE_BUILD_TYPE)
set(CMAKE_BUILD_TYPE Debug) set(CMAKE_BUILD_TYPE Release)
endif() endif()
if (CMAKE_BUILD_TYPE MATCHES "Debug") if (CMAKE_BUILD_TYPE MATCHES "Debug")
@ -13,6 +13,10 @@ else()
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17 -O3 -DNDEBUG -fPIC") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17 -O3 -DNDEBUG -fPIC")
endif() endif()
if(POLICY CMP0167)
cmake_policy(SET CMP0167 NEW)
endif()
find_package(Boost COMPONENTS program_options REQUIRED) find_package(Boost COMPONENTS program_options REQUIRED)
include_directories( include_directories(
@ -22,9 +26,10 @@ include_directories(
) )
link_libraries(unitree_sdk2 ddsc ddscxx rt pthread) 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 # example
add_executable(h1_hand_example example/inspire_hand.cpp) add_executable(hand_example example/hand_example.cpp)

121
README.md Normal file
View File

@ -0,0 +1,121 @@
<div align="center">
<h1 align="center">DFX Inspire Hand service</h1>
<a href="https://www.unitree.com/" target="_blank">
<img src="https://www.unitree.com/images/0079f8938336436e955ea3a98c4e1e59.svg" alt="Unitree LOGO" width="15%">
</a>
</div>
# 1. 📦 Introduction
[Unitree Robot RH56DFX Inspire Hand](https://support.unitree.com/home/en/H1_developer/Dexterous_hand) Controller.
<div align="center">
<img src="doc/img/RH56.png" width="300"/>
</div>
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.
<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>
# 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.

BIN
bin/x86_64/h1_hand_example
View File

Binary file not shown.

BIN
bin/x86_64/inspire_hand
View File

Binary file not shown.

80
doc/Inspire-Hand.md
View File

@ -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<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>

80
doc/灵巧手开发.md
View File

@ -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<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>

0
example/inspire_hand.cpp → example/hand_example.cpp
View File

41
include/SerialPort.h
View File

@ -14,15 +14,6 @@
#include <chrono> #include <chrono>
#include <queue> #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 class SerialPort
{ {
public: public:
@ -41,9 +32,7 @@ public:
ssize_t send(const uint8_t* data, size_t len) ssize_t send(const uint8_t* data, size_t len)
{ {
// tcflush(fd_, TCIFLUSH);
ssize_t ret = ::write(fd_, data, len); ssize_t ret = ::write(fd_, data, len);
// tcdrain(fd_);
return ret; return ret;
} }
@ -69,36 +58,6 @@ public:
return recv_len; 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) void set_timeout(int timeout_ms)
{ {
timeout_.tv_sec = timeout_ms / 1000; timeout_.tv_sec = timeout_ms / 1000;

172
include/dds/Publisher.h Normal file
View File

@ -0,0 +1,172 @@
#ifndef _UT_ROBOT_PUBLISHER_H_
#define _UT_ROBOT_PUBLISHER_H_
#include <unitree/robot/channel/channel_publisher.hpp>
#include <atomic>
#include <thread>
#include <memory>
namespace unitree
{
namespace robot
{
template <typename MessageType>
class PublisherBase : public unitree::robot::ChannelPublisher<MessageType>
{
public:
using MsgType = MessageType;
using SharedPtr = std::shared_ptr<PublisherBase<MsgType>>;
PublisherBase(std::string TOPIC_NAME)
: unitree::robot::ChannelPublisher<MessageType>(TOPIC_NAME)
{
this->InitChannel();
}
};
// For details: see https://github.com/ros-controls/realtime_tools
template <typename MessageType>
class RealTimePublisher
{
public:
using MsgType = MessageType;
using PublisherSharedPtr = typename unitree::robot::ChannelPublisherPtr<MessageType>;
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<PublisherBase<MsgType>>(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<int> turn_;
};
} // namespace robot
} // namespace unitree
#endif // _UT_ROBOT_PUBLISHER_H_

54
include/dds/Subscription.h Normal file
View File

@ -0,0 +1,54 @@
#ifndef _UT_ROBOT_SUBSCRIPTION_H_
#define _UT_ROBOT_SUBSCRIPTION_H_
#include <unitree/robot/channel/channel_subscriber.hpp>
#include <mutex>
namespace unitree
{
namespace robot
{
template <typename MessageType>
class SubscriptionBase
{
public:
using MsgType = MessageType;
using SharedPtr = std::shared_ptr<SubscriptionBase<MsgType>>;
SubscriptionBase(const std::string& topic)
{
last_update_time_ = std::chrono::steady_clock::now() - std::chrono::milliseconds(timeout_ms_);
sub_ = std::make_shared<unitree::robot::ChannelSubscriber<MessageType>>(topic);
sub_->InitChannel([this](const void *msg){
last_update_time_ = std::chrono::steady_clock::now();
std::lock_guard<std::mutex> 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<MessageType> sub_;
std::chrono::steady_clock::time_point last_update_time_;
};
}; // namespace robot
}; // namespace unitree
#endif // _UT_ROBOT_SUBSCRIPTION_H_

42
include/inspire.h
View File

@ -32,17 +32,15 @@ public:
/** /**
* @brief 设置各自由度的位置 * @brief Set the finger position
* -1: 不变 * -1: no change
* [0, 1] 0: 闭合 * [0, 1] 0: close
* 小拇指、无名指、中指、食指、大拇指弯曲、大拇指旋转 * ID: pinky, ring, middle, index, thumb_bend, thumb_rotate
*/ */
int16_t SetPosition(const Eigen::Matrix<double, 6, 1> & q) 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); Eigen::Matrix<int16_t, 6, 1> q_int16 = (q * 1000).cast<int16_t>().cwiseMax(0).cwiseMin(1000);
uint8_t cmd[20]; uint8_t cmd[20];
cmd[0] = 0xEB; // head cmd[0] = 0xEB; // head
cmd[1] = 0x90; 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<double, 6, 1> & q) int16_t GetPosition(Eigen::Matrix<double, 6, 1> & 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) 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 * [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<double, 6, 1> & f) int16_t GetForce(Eigen::Matrix<double, 6, 1> & 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() 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() void Calibration()
{ {
@ -234,11 +231,9 @@ public:
serial_->send(cmd.data(), cmd.size()); serial_->send(cmd.data(), cmd.size());
usleep(5000); usleep(5000);
serial_->recv(recvBuff, 9); // 第一帧 serial_->recv(recvBuff, 9); // First frame
sleep(10); // 整个过程大约需要6s sleep(10); // The calibration process takes about 6 seconds
serial_->recv(recvBuff, 9); // 第二帧 serial_->recv(recvBuff, 9); // Second frame
std::cout << "Calibration: ";
print_data(recvBuff, 9);
} }
uint8_t id = 1; uint8_t id = 1;
@ -246,7 +241,6 @@ private:
uint8_t CheckSum(const uint8_t* data, uint8_t len) uint8_t CheckSum(const uint8_t* data, uint8_t len)
{ {
uint8_t sum = 0; uint8_t sum = 0;
// 除应答帧头外其余数据的累加和的低字节
for (int i = 2; i < len - 1; i++) for (int i = 2; i < len - 1; i++)
{ {
sum += data[i]; sum += data[i];

2
include/param.h
View File

@ -29,7 +29,7 @@ po::variables_map helper(int argc, char** argv)
po::options_description desc("Unitree H1 Inspire Hand Serial to DDS"); po::options_description desc("Unitree H1 Inspire Hand Serial to DDS");
desc.add_options() desc.add_options()
("help,h", "produce help message") ("help,h", "produce help message")
("serial,s", po::value<std::string>(&serial_port)->default_value("/dev/h1_newhand"), "serial port") ("serial,s", po::value<std::string>(&serial_port)->default_value("/dev/ttyUSB0"), "serial port")
("network", po::value<std::string>(&network)->default_value(""), "DDS network interface") ("network", po::value<std::string>(&network)->default_value(""), "DDS network interface")
("namespace", po::value<std::string>(&ns)->default_value("inspire"), "DDS topic namespace") ("namespace", po::value<std::string>(&ns)->default_value("inspire"), "DDS topic namespace")
; ;

114
inspire_g1.cpp Normal file
View File

@ -0,0 +1,114 @@
#include "inspire.h"
#include "param.h"
#include "dds/Publisher.h"
#include "dds/Subscription.h"
#include <unitree/idl/go2/MotorCmds_.hpp>
#include <unitree/idl/go2/MotorStates_.hpp>
#include <unitree/common/thread/recurrent_thread.hpp>
class InspireRunner
{
public:
InspireRunner()
{
serial1 = std::make_shared<SerialPort>("/dev/ttyUSB1", B115200);
serial2 = std::make_shared<SerialPort>("/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<inspire::InspireHand>(serial1, 1);
lefthand = std::make_shared<inspire::InspireHand>(serial2, 1);
// 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::RealTimePublisher<unitree_go::msg::dds_::MotorStates_>>(
"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 serial1;
SerialPort::SharedPtr serial2;
std::shared_ptr<inspire::InspireHand> lefthand;
std::shared_ptr<inspire::InspireHand> righthand;
Eigen::Matrix<double, 12, 1> qcmd, qstate;
// dds
std::unique_ptr<unitree::robot::RealTimePublisher<unitree_go::msg::dds_::MotorStates_>> handstate;
std::shared_ptr<unitree::robot::SubscriptionBase<unitree_go::msg::dds_::MotorCmds_>> 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;
}

32
inspire_ctrl.cpp → inspire_h1.cpp
View File

@ -1,8 +1,12 @@
#include "inspire.h" #include "inspire.h"
#include <unitree/dds_wrapper/go2/go2_pub.h>
#include <unitree/dds_wrapper/go2/go2_sub.h>
#include "param.h" #include "param.h"
#include "dds/Publisher.h"
#include "dds/Subscription.h"
#include <unitree/idl/go2/MotorCmds_.hpp>
#include <unitree/idl/go2/MotorStates_.hpp>
#include <unitree/common/thread/recurrent_thread.hpp>
class InspireRunner class InspireRunner
{ {
public: public:
@ -11,14 +15,14 @@ public:
serial = std::make_shared<SerialPort>(param::serial_port, B115200); serial = std::make_shared<SerialPort>(param::serial_port, B115200);
// inspire // inspire
righthand = std::make_shared<inspire::InspireHand>(serial, 1); // 按邵博格式，先右后左 righthand = std::make_shared<inspire::InspireHand>(serial, 1); // ID 1
lefthand = std::make_shared<inspire::InspireHand>(serial, 2); lefthand = std::make_shared<inspire::InspireHand>(serial, 2); // ID 2
// dds // dds
handcmd = std::make_shared<unitree::robot::SubscriptionBase<unitree_go::msg::dds_::MotorCmds_>>( handcmd = std::make_shared<unitree::robot::SubscriptionBase<unitree_go::msg::dds_::MotorCmds_>>(
"rt/" + param::ns + "/cmd"); "rt/" + param::ns + "/cmd");
handcmd->msg_.cmds().resize(12); handcmd->msg_.cmds().resize(12);
handstate = std::make_unique<unitree::robot::go2::publisher::RealTimeMotorStates>( handstate = std::make_unique<unitree::robot::RealTimePublisher<unitree_go::msg::dds_::MotorStates_>>(
"rt/" + param::ns + "/state"); "rt/" + param::ns + "/state");
handstate->msg_.states().resize(12); handstate->msg_.states().resize(12);
@ -37,7 +41,7 @@ public:
{ {
qcmd(i) = handcmd->msg_.cmds()[i].q(); 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)); lefthand->SetPosition(qcmd.block<6, 1>(6, 0));
} }
@ -53,7 +57,7 @@ public:
{ {
handstate->msg_.states()[i].lost()++; 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) if(lefthand->GetPosition(qtemp) == 0)
{ {
@ -65,7 +69,7 @@ public:
{ {
handstate->msg_.states()[i+6].lost()++; 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()) if(handstate->trylock())
{ {
@ -86,22 +90,14 @@ public:
Eigen::Matrix<double, 12, 1> qcmd, qstate; Eigen::Matrix<double, 12, 1> qcmd, qstate;
// dds // dds
std::unique_ptr<unitree::robot::RealTimePublisher<unitree_go::msg::dds_::MotorStates_>> handstate;
std::shared_ptr<unitree::robot::SubscriptionBase<unitree_go::msg::dds_::MotorCmds_>> handcmd; 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) int main(int argc, char ** argv)
{ {
auto vm = param::helper(argc, argv); auto vm = param::helper(argc, argv);
if (param::network.empty()) unitree::robot::ChannelFactory::Instance()->Init(0, param::network);
{
// 在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 << " --- Unitree Robotics --- " << std::endl;
std::cout << " Inspire Hand Controller " << std::endl; std::cout << " Inspire Hand Controller " << std::endl;