Skip to content

Commit b52c204

Browse files
Agnel-WangAgnel-Wang
committed
initial release
0 parents  commit b52c204

File tree

13 files changed

+951
-0
lines changed

13 files changed

+951
-0
lines changed

‎.vscode/settings.json‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
{
2+
"ros.distro": "noetic"
3+
}

‎CHANGELOG.md‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
# Changelog
2+
3+
## 1.0.1
4+
5+
+ Initial release.

‎CMakeLists.txt‎

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
1+
cmake_minimum_required(VERSION 3.10)
2+
project(h1_inspire_service VERSION 0.1.0)
3+
4+
if(NOT CMAKE_BUILD_TYPE)
5+
set(CMAKE_BUILD_TYPE Debug)
6+
endif()
7+
8+
if (CMAKE_BUILD_TYPE MATCHES "Debug")
9+
message("Debug mode")
10+
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17 -g -O0 -fPIC")
11+
else()
12+
message("Release mode")
13+
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17 -O3 -DNDEBUG -fPIC")
14+
endif()
15+
16+
find_package(Boost COMPONENTS program_options REQUIRED)
17+
18+
include_directories(
19+
/usr/local/include/ddscxx
20+
/usr/local/include/iceoryx/v2.0.2
21+
include
22+
)
23+
24+
link_libraries(unitree_sdk2 ddsc ddscxx rt pthread)
25+
link_libraries(unitree_idl libboost_program_options.a)
26+
27+
add_executable(inspire_hand inspire_ctrl.cpp)
28+
29+
# example
30+
add_executable(h1_hand_example example/inspire_hand.cpp)

‎bin/x86_64/h1_hand_example‎

2.64 MB
Binary file not shown.

‎bin/x86_64/inspire_hand‎

3.7 MB
Binary file not shown.

‎doc/Inspire-Hand.md‎

Lines changed: 80 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,80 @@
1+
## Dexterous Hand
2+
3+
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.
4+
5+
![RH56](./img/RH56.png)
6+
7+
##
8+
9+
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"**.
10+
11+
```mermaid
12+
graph LR
13+
A(user) --rt/inspire/cmd--> B(H1)
14+
B --rt/inspire/state--> A
15+
```
16+
17+
+ IDL Message Type
18+
19+
Motor data in array format, containing 12 motor data for both hands.
20+
21+
!!!note
22+
Currently the dexterous hand only supports joint control, i.e. only the parameter q makes sense in the idl format. The others are reserved.
23+
!!!
24+
25+
26+
```bash
27+
# namespace unitree_go::msg::dds_
28+
29+
# unitree_go::msg::dds_::MotorCmds_
30+
struct MotorCmds_
31+
{
32+
sequence<unitree_go::msg::dds_::MotorCmd_> cmds;
33+
};
34+
35+
# unitree_go::msg::dds_::MotorStates_
36+
struct MotorCmds_
37+
{
38+
sequence<unitree_go::msg::dds_::MotorState> states;
39+
};
40+
```
41+
+ Joint Motor Sequence
42+
43+
<div style="text-align: center;">
44+
<table border="1">
45+
<tr>
46+
<td>Id</td>
47+
<td>0</td>
48+
<td>1</td>
49+
<td>2</td>
50+
<td>3</td>
51+
<td>4</td>
52+
<td>5</td>
53+
<td>6</td>
54+
<td>7</td>
55+
<td>8</td>
56+
<td>9</td>
57+
<td>10</td>
58+
<td>11</td>
59+
</tr>
60+
<tr>
61+
<td rowspan="2">Joint</td>
62+
<td colspan="6">Right Hand</td>
63+
<td colspan="6">Left Hand</td>
64+
</tr>
65+
<tr>
66+
<td>pinky</td>
67+
<td>ring</td>
68+
<td>middle</td>
69+
<td>index</td>
70+
<td>thumb-bend</td>
71+
<td>thumb-rotation</td>
72+
<td>pinky</td>
73+
<td>ring</td>
74+
<td>middle</td>
75+
<td>index</td>
76+
<td>thumb-bend</td>
77+
<td>thumb-rotation</td>
78+
</tr>
79+
</table>
80+
</div>

‎doc/img/RH56.png‎

89.7 KB
Loading

‎doc/灵巧手开发.md‎

Lines changed: 80 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,80 @@
1+
## 灵巧手介绍
2+
3+
H1可搭载[Inspire Robotics](https://inspire-robots.com/product/frwz/)的仿人五指灵巧手，该灵巧手具有6个自由度和12个运动关节，可以模拟人手实现复杂动作。
4+
![RH56](./img/RH56.png)
5+
6+
## 接口说明
7+
8+
**示例程序** [h1_example_hand.cpp]()
9+
10+
用户向 **"rt/inspire/cmd"** 话题发送 **"unitree_go::msg::dds::MotorCmds_"** 消息控制灵巧手。
11+
从 **"rt/inspire/state"** 话题接受 **"unitree_go::msg::dds::MotorStates_"** 消息获取灵巧手状态。
12+
13+
```mermaid
14+
graph LR
15+
A(user) --rt/inspire/cmd--> B(H1)
16+
B --rt/inspire/state--> A
17+
```
18+
19+
+ IDL数据格式
20+
采用数组格式的电机数据，内部包含双手12个电机数据。
21+
```note
22+
当前灵巧手只支持关节控制，即在idl格式中只有参数q有意义。其他保留。
23+
```
24+
25+
```bash
26+
# namespace unitree_go::msg::dds_
27+
28+
# unitree_go::msg::dds_::MotorCmds_
29+
struct MotorCmds_
30+
{
31+
sequence<unitree_go::msg::dds_::MotorCmd_> cmds;
32+
};
33+
34+
# unitree_go::msg::dds_::MotorStates_
35+
struct MotorCmds_
36+
{
37+
sequence<unitree_go::msg::dds_::MotorState> states;
38+
};
39+
```
40+
41+
42+
+ IDL中的关节顺序
43+
<div style="text-align: center;">
44+
<table border="1">
45+
<tr>
46+
<td>Id</td>
47+
<td>0</td>
48+
<td>1</td>
49+
<td>2</td>
50+
<td>3</td>
51+
<td>4</td>
52+
<td>5</td>
53+
<td>6</td>
54+
<td>7</td>
55+
<td>8</td>
56+
<td>9</td>
57+
<td>10</td>
58+
<td>11</td>
59+
</tr>
60+
<tr>
61+
<td rowspan="2">Joint</td>
62+
<td colspan="6">Right Hand</td>
63+
<td colspan="6">Left Hand</td>
64+
</tr>
65+
<tr>
66+
<td>pinky</td>
67+
<td>ring</td>
68+
<td>middle</td>
69+
<td>index</td>
70+
<td>thumb-bend</td>
71+
<td>thumb-rotation</td>
72+
<td>pinky</td>
73+
<td>ring</td>
74+
<td>middle</td>
75+
<td>index</td>
76+
<td>thumb-bend</td>
77+
<td>thumb-rotation</td>
78+
</tr>
79+
</table>
80+
</div>

‎example/inspire_hand.cpp‎

Lines changed: 163 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,163 @@
1+
/**
2+
* @file inspire_hand.cpp
3+
* @brief This is an example of how to control the Unitree H1 (Inspire) Hand using unitree_sdk2.
4+
*/
5+
6+
// Inspire Hand Topic IDL Types
7+
#include <unitree/idl/go2/MotorCmds_.hpp>
8+
#include <unitree/idl/go2/MotorStates_.hpp>
9+
// DDS Channel
10+
#include <unitree/robot/channel/channel_publisher.hpp>
11+
#include <unitree/robot/channel/channel_subscriber.hpp>
12+
#include <unitree/common/thread/thread.hpp>
13+
14+
#include <eigen3/Eigen/Dense>
15+
#include <unordered_map>
16+
17+
/**
18+
* @brief Unitree H1 Hand Controller
19+
* 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.
20+
*
21+
* IDL Types
22+
* user ---(unitree_go::msg::dds_::MotorCmds_)---> "rt/inspire/cmd"
23+
* user <--(unitree_go::msg::dds_::MotorStates_)-- "rt/inspire/state"
24+
*
25+
* @attention Currently the hand only supports position control, which means only the `q` field in idl is used.
26+
*/
27+
class H1HandController
28+
{
29+
public:
30+
H1HandController()
31+
{
32+
this->InitDDS_();
33+
}
34+
35+
/**
36+
* @brief Control the hand to a specific label
37+
*/
38+
void ctrl(std::string label)
39+
{
40+
if(labels.find(label) != labels.end())
41+
{
42+
this->ctrl(labels[label], labels[label]);
43+
}
44+
else
45+
{
46+
std::cout << "Invalid label: " << label << std::endl;
47+
}
48+
}
49+
50+
/**
51+
* @brief Move the fingers to the specified angles
52+
*
53+
* @note The angles should be in the range [0, 1]
54+
* 0: close 1: open
55+
*/
56+
void ctrl(
57+
const Eigen::Matrix<float, 6, 1>& right_angles,
58+
const Eigen::Matrix<float, 6, 1>& left_angles)
59+
{
60+
for(size_t i(0); i<6; i++)
61+
{
62+
cmd.cmds()[i].q() = right_angles(i);
63+
cmd.cmds()[i+6].q() = left_angles(i);
64+
}
65+
handcmd->Write(cmd);
66+
}
67+
68+
/**
69+
* @brief Get the right hand angles
70+
*
71+
* Joint order: [pinky, ring, middle, index, thumb_bend, thumb_rotation]
72+
*/
73+
Eigen::Matrix<float, 6, 1> getRightQ()
74+
{
75+
std::lock_guard<std::mutex> lock(mtx);
76+
Eigen::Matrix<float, 6, 1> q;
77+
for(size_t i(0); i<6; i++)
78+
{
79+
q(i) = state.states()[i].q();
80+
}
81+
return q;
82+
}
83+
84+
/**
85+
* @brief Get the left hand angles
86+
*
87+
* Joint order: [pinky, ring, middle, index, thumb_bend, thumb_rotation]
88+
*/
89+
Eigen::Matrix<float, 6, 1> getLeftQ()
90+
{
91+
std::lock_guard<std::mutex> lock(mtx);
92+
Eigen::Matrix<float, 6, 1> q;
93+
for(size_t i(0); i<6; i++)
94+
{
95+
q(i) = state.states()[i+6].q();
96+
}
97+
return q;
98+
}
99+
100+
unitree_go::msg::dds_::MotorCmds_ cmd;
101+
unitree_go::msg::dds_::MotorStates_ state;
102+
private:
103+
void InitDDS_()
104+
{
105+
handcmd = std::make_shared<unitree::robot::ChannelPublisher<unitree_go::msg::dds_::MotorCmds_>>(
106+
"rt/inspire/cmd");
107+
handcmd->InitChannel();
108+
cmd.cmds().resize(12);
109+
handstate = std::make_shared<unitree::robot::ChannelSubscriber<unitree_go::msg::dds_::MotorStates_>>(
110+
"rt/inspire/state");
111+
handstate->InitChannel([this](const void *message){
112+
std::lock_guard<std::mutex> lock(mtx);
113+
state = *(unitree_go::msg::dds_::MotorStates_*)message;
114+
});
115+
state.states().resize(12);
116+
}
117+
118+
// DDS parameters
119+
std::mutex mtx;
120+
unitree::robot::ChannelPublisherPtr<unitree_go::msg::dds_::MotorCmds_> handcmd;
121+
unitree::robot::ChannelSubscriberPtr<unitree_go::msg::dds_::MotorStates_> handstate;
122+
123+
// Saved labels
124+
std::unordered_map<std::string, Eigen::Matrix<float, 6, 1>> labels = {
125+
{"open", Eigen::Matrix<float, 6, 1>::Ones()},
126+
{"close", Eigen::Matrix<float, 6, 1>::Zero()},
127+
{"half", Eigen::Matrix<float, 6, 1>::Constant(0.5)},
128+
};
129+
};
130+
131+
/**
132+
* Main Function
133+
*/
134+
int main(int argc, char** argv)
135+
{
136+
std::cout << " --- Unitree Robotics --- \n";
137+
std::cout << " H1 Hand Example \n\n";
138+
139+
// Target label
140+
std::string label = "close"; // You change this value to other labels
141+
142+
// Initialize the DDS Channel
143+
std::string networkInterface = argc > 1 ? argv[1] : "";
144+
unitree::robot::ChannelFactory::Instance()->Init(0, networkInterface);
145+
146+
// Create the H1 Hand Controller
147+
auto h1hand = std::make_shared<H1HandController>();
148+
149+
int cnt = 0;
150+
while (true)
151+
{
152+
usleep(100000);
153+
if(cnt++ % 10 == 0)
154+
label = label == "close" ? "open" : "close";
155+
h1hand->ctrl(label); // Control the hand
156+
std::cout << "-- Hand State --\n";
157+
std::cout << " R: " << h1hand->getRightQ().transpose() << std::endl;
158+
std::cout << " L: " << h1hand->getLeftQ().transpose() << std::endl;
159+
std::cout << "\033[3A"; // Move cursor up 3 lines
160+
}
161+
162+
return 0;
163+
}

0 commit comments

Comments
 (0)