initial release
This commit is contained in:
Agnel Wang
commit
b52c204997
3
.vscode/settings.json
vendored
Normal file
3
.vscode/settings.json
vendored
Normal file
@ -0,0 +1,3 @@
|
||||
{
|
||||
"ros.distro": "noetic"
|
||||
}
|
||||
5
CHANGELOG.md
Normal file
5
CHANGELOG.md
Normal file
@ -0,0 +1,5 @@
|
||||
# Changelog
|
||||
|
||||
## 1.0.1
|
||||
|
||||
+ Initial release.
|
||||
30
CMakeLists.txt
Normal file
30
CMakeLists.txt
Normal file
@ -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)
|
||||
BIN
bin/x86_64/h1_hand_example
Executable file
BIN
bin/x86_64/h1_hand_example
Executable file
Binary file not shown.
BIN
bin/x86_64/inspire_hand
Executable file
BIN
bin/x86_64/inspire_hand
Executable file
Binary file not shown.
80
doc/Inspire-Hand.md
Normal file
80
doc/Inspire-Hand.md
Normal file
@ -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.
|
||||
|
||||
|
||||
|
||||
##
|
||||
|
||||
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>
|
||||
BIN
doc/img/RH56.png
Normal file
BIN
doc/img/RH56.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 90 KiB |
80
doc/灵巧手开发.md
Normal file
80
doc/灵巧手开发.md
Normal file
@ -0,0 +1,80 @@
|
||||
## 灵巧手介绍
|
||||
|
||||
H1可搭载[Inspire Robotics](https://inspire-robots.com/product/frwz/)的仿人五指灵巧手，该灵巧手具有6个自由度和12个运动关节，可以模拟人手实现复杂动作。
|
||||
|
||||
|
||||
## 接口说明
|
||||
|
||||
**示例程序** [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>
|
||||
163
example/inspire_hand.cpp
Normal file
163
example/inspire_hand.cpp
Normal file
@ -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;
|
||||
}
|
||||
154
include/SerialPort.h
Normal file
154
include/SerialPort.h
Normal file
@ -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
|
||||
262
include/inspire.h
Normal file
262
include/inspire.h
Normal file
@ -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
|
||||
58
include/param.h
Normal file
58
include/param.h
Normal file
@ -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
|
||||
116
inspire_ctrl.cpp
Normal file
116
inspire_ctrl.cpp
Normal file
@ -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;
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user