feat: 初始化 Go2 导航相关代码与配置
This commit is contained in:
18
unitree_sdk2/example/h1/CMakeLists.txt
Normal file
18
unitree_sdk2/example/h1/CMakeLists.txt
Normal file
@@ -0,0 +1,18 @@
|
||||
add_executable(h1_loco_client high_level/h1_loco_client_example.cpp)
|
||||
target_link_libraries(h1_loco_client unitree_sdk2)
|
||||
|
||||
add_executable(h1_low_level_example low_level/humanoid.cpp)
|
||||
target_link_libraries(h1_low_level_example unitree_sdk2)
|
||||
|
||||
add_executable(h1_27dof_example low_level/h1_27dof_example.cpp)
|
||||
target_link_libraries(h1_27dof_example unitree_sdk2)
|
||||
|
||||
add_executable(h1_2_ankle_track low_level/h1_2_ankle_track.cpp)
|
||||
target_link_libraries(h1_2_ankle_track unitree_sdk2)
|
||||
|
||||
add_executable(h1_arm_sdk_dds_example high_level/h1_arm_sdk_dds_example.cpp)
|
||||
target_link_libraries(h1_arm_sdk_dds_example unitree_sdk2)
|
||||
|
||||
add_executable(h1_2_arm_sdk_dds_example high_level/h1_2_arm_sdk_dds_example.cpp)
|
||||
target_link_libraries(h1_2_arm_sdk_dds_example unitree_sdk2)
|
||||
|
||||
111
unitree_sdk2/example/h1/README.md
Normal file
111
unitree_sdk2/example/h1/README.md
Normal file
@@ -0,0 +1,111 @@
|
||||
# Unitree H1_2 Parallel Mechanism Control
|
||||
|
||||
English | [简体中文](README_zh.md)
|
||||
|
||||
## Parallel Mechanism Control Interface
|
||||
|
||||
The Unitree H1_2 robot's parallel mechanism involves the ankle joints of its left and right legs. Since the left and right ankle joints are symmetrical, the following explanation will use the H1_2 left ankle joint as an example:
|
||||
|
||||
<p align="center"><img src="doc/images/ankle.png" width="30%"/></p>
|
||||
|
||||
On the hardware side, the H1_2 left ankle joint uses a parallel mechanism consisting of four joints:
|
||||
|
||||
* Parallel joints: Joint A, Joint B
|
||||
* Serial joints: Pitch joint (P joint), Roll joint (R joint)
|
||||
|
||||
Only the A and B joints can be directly driven by motors, while the P and R joints cannot be directly controlled. The robot's URDF model and motion control algorithms typically only consider the serial P and R joints. To control these joints, we control the A and B joints indirectly, which allows us to achieve control over the P and R joints. On the software side, we provide an equivalent serial joint control interface, called `PR Mode`, enabling users to directly control the P and R joints. Similar to the typical joint control method, in `PR Mode`, each serial joint receives the following commands:
|
||||
|
||||
| Command Name | Variable |
|
||||
| ----------------------- | -------- |
|
||||
| Feedforward Torque | `tau` |
|
||||
| Target Angle | `q` |
|
||||
| Target Angular Velocity | `dq` |
|
||||
| Joint Stiffness | `kp` |
|
||||
| Joint Damping | `kd` |
|
||||
|
||||
The total torque applied to the serial joint is calculated as `T = kp * (q - q_m) + kd * (dq - dq_m) + tau`. To improve the control precision of the serial joints, the H1_2 robot internally converts the feedforward torque, target angle, target angular velocity, joint stiffness, and joint damping commands for the P and R joints into actual commands for the A and B joints based on kinematic and dynamic relationships.
|
||||
|
||||
## Serial Joint Tracking Experiment
|
||||
|
||||
To test the performance of the H1_2 ankle in `PR Mode`, we have the P and R joints track a sinusoidal curve. You can refer to the [example](https://github.com/unitreerobotics/unitree_sdk2/blob/main/example/h1/low_level/h1_2_ankle_track.cpp). The core code is as follows:
|
||||
|
||||
**Enabling PR Mode and Generating Sinusoidal Curve**
|
||||
|
||||
```c++
|
||||
// [Stage 2]: swing ankle's PR
|
||||
mode_ = PR; // Enable PR mode
|
||||
// generate sin/cos trajectory
|
||||
double max_P = 0.25; // [rad]
|
||||
double max_R = 0.25; // [rad]
|
||||
double t = time_ - duration_;
|
||||
double L_P_des = max_P * std::cos(2.0 * M_PI * t);
|
||||
double L_R_des = max_R * std::sin(2.0 * M_PI * t);
|
||||
double R_P_des = max_P * std::cos(2.0 * M_PI * t);
|
||||
double R_R_des = -max_R * std::sin(2.0 * M_PI * t);
|
||||
```
|
||||
|
||||
**Setting Ankle Joint Commands**
|
||||
|
||||
```c++
|
||||
// update ankle joint position targets
|
||||
float Kp_Pitch = 80;
|
||||
float Kd_Pitch = 1;
|
||||
float Kp_Roll = 80;
|
||||
float Kd_Roll = 1;
|
||||
dds_low_command.motor_cmd().at(4).q() = L_P_des; // 4: LeftAnklePitch
|
||||
dds_low_command.motor_cmd().at(4).dq() = 0;
|
||||
dds_low_command.motor_cmd().at(4).kp() = Kp_Pitch;
|
||||
dds_low_command.motor_cmd().at(4).kd() = Kd_Pitch;
|
||||
dds_low_command.motor_cmd().at(4).tau() = 0;
|
||||
dds_low_command.motor_cmd().at(5).q() = L_R_des; // 5: LeftAnkleRoll
|
||||
dds_low_command.motor_cmd().at(5).dq() = 0;
|
||||
dds_low_command.motor_cmd().at(5).kp() = Kp_Roll;
|
||||
dds_low_command.motor_cmd().at(5).kd() = Kd_Roll;
|
||||
dds_low_command.motor_cmd().at(5).tau() = 0;
|
||||
dds_low_command.motor_cmd().at(10).q() = R_P_des; // 10: RightAnklePitch
|
||||
dds_low_command.motor_cmd().at(10).dq() = 0;
|
||||
dds_low_command.motor_cmd().at(10).kp() = Kp_Pitch;
|
||||
dds_low_command.motor_cmd().at(10).kd() = Kd_Pitch;
|
||||
dds_low_command.motor_cmd().at(10).tau() = 0;
|
||||
dds_low_command.motor_cmd().at(11).q() = R_R_des; // 11: RightAnkleRoll
|
||||
dds_low_command.motor_cmd().at(11).dq() = 0;
|
||||
dds_low_command.motor_cmd().at(11).kp() = Kp_Roll;
|
||||
dds_low_command.motor_cmd().at(11).kd() = Kd_Roll;
|
||||
dds_low_command.motor_cmd().at(11).tau() = 0;
|
||||
```
|
||||
|
||||
**Printing Desired and Measured Values to Terminal**
|
||||
|
||||
```c++
|
||||
float L_P_m = low_state_.motor_state().at(4).q();
|
||||
float L_R_m = low_state_.motor_state().at(5).q();
|
||||
float R_P_m = low_state_.motor_state().at(10).q();
|
||||
float R_R_m = low_state_.motor_state().at(11).q();
|
||||
printf("%f,%f,%f,%f,%f,%f,%f,%f\n", L_P_des, L_P_m, L_R_des, L_R_m, R_P_des, R_P_m, R_R_des, R_R_m);
|
||||
```
|
||||
|
||||
| L_P_des | L_P_m | L_R_des | L_R_m | R_P_des | R_P_m | R_R_des | R_R_m |
|
||||
| ------------------ | ------------------- | ----------------- | ------------------ | ------------------- | -------------------- | ------------------ | ------------------- |
|
||||
| Left Pitch Desired | Left Pitch Measured | Left Roll Desired | Left Roll Measured | Right Pitch Desired | Right Pitch Measured | Right Roll Desired | Right Roll Measured |
|
||||
|
||||
To install and compile [unitree_sdk2](https://github.com/unitreerobotics/unitree_sdk2), ensure that the robot is **suspended first**, then run the test routine in the terminal:
|
||||
|
||||
```bash
|
||||
h1_2_ankle_track network_interface
|
||||
```
|
||||
|
||||
Once started, the robot will first reset to the zero position, then periodically swing its ankles while printing the desired and measured values for both ankle joints. The position tracking results are shown below:
|
||||
|
||||
<p float="middle">
|
||||
<img src="doc/images/tracking.png" width="49%"/>
|
||||
<img src="doc/images/tracking_circle.png" width="49%"/>
|
||||
</p>
|
||||
|
||||
Definitions of the symbols in the figures:
|
||||
|
||||
* `L_Pitch_d`: Desired value of the left ankle's P joint
|
||||
* `L_Pitch_m`: Measured value of the left ankle's P joint
|
||||
* `L_Roll_d`: Desired value of the left ankle's R joint
|
||||
* `L_Roll_m`: Measured value of the left ankle's R joint
|
||||
|
||||
The left graph shows that the serial Pitch and Roll joints can accurately track the sinusoidal target position commands, while the right graph illustrates the tracking performance of the serial joints in phase space.
|
||||
111
unitree_sdk2/example/h1/README_zh.md
Normal file
111
unitree_sdk2/example/h1/README_zh.md
Normal file
@@ -0,0 +1,111 @@
|
||||
# Unitree H1_2 并联机构控制
|
||||
|
||||
[English](README.md) | 简体中文
|
||||
|
||||
## 并联机构控制接口
|
||||
|
||||
Unitree H1_2 机器人并联机构涉及机器人的左右腿的脚踝。因左右脚踝对称，故下面以 H1_2 左脚踝关节为例：
|
||||
|
||||
<p align="center"><img src="doc/images/ankle.png" width="30%"/></p>
|
||||
|
||||
硬件上，H1_2 左脚踝关节采用并联机构，包括四个关节：
|
||||
|
||||
* 并联关节：A 关节、B 关节
|
||||
* 串联关节：Pitch 关节 (简称 P 关节)、Roll 关节 (简称 R 关)
|
||||
|
||||
其中只有 A、B 关节是可被电机直接驱动的关节，P、R 关节不能被直接控制。而机器人 URDF 模型和运动控制算法通常只考虑 P、R 串联关节。为了实现对 P、R 关节的控制，我们通过控制 A、B 关节从而间接实现对 P、R 关节的控制。具体到软件上，我们为用户提供了等价的串联关节控制接口，即 `PR 模式`，让用户能直接控制 P、R 关节。与普通关节控制方法一样，`PR 模式` 下每个串联关节接收以下指令：
|
||||
|
||||
| 指令名称 | 变量 |
|
||||
| ---------- | ----- |
|
||||
| 前馈力矩 | `tau` |
|
||||
| 目标角度 | `q` |
|
||||
| 目标角速度 | `dq` |
|
||||
| 关节刚度 | `kp` |
|
||||
| 关节阻尼 | `kd` |
|
||||
|
||||
最终串联关节执行总的力矩为 `T = kp * (q - q_m) + kd * (dq - dq_m) + tau`。为了提高串联关节控制精度，H1_2 机器人内部根据运动学和动力学关系，把 P、R 关节的前馈力矩、目标角度、目标角速度、关节刚度和关节阻尼指令转换为 A、B 实际关节执行单元。
|
||||
|
||||
## 串联关节跟踪实验
|
||||
|
||||
为测试 H1_2 脚踝 `PR 模式` 控制效果，我们让脚踝 P、R 关节跟踪正弦曲线，参考[测试例程](https://github.com/unitreerobotics/unitree_sdk2/blob/main/example/h1/low_level/h1_2_ankle_track.cpp)。核心代码段如下：
|
||||
|
||||
**启用 PR 模式并生成正弦曲线**
|
||||
|
||||
```c++
|
||||
// [Stage 2]: swing ankle's PR
|
||||
mode_ = PR; // Enable PR mode
|
||||
// generate sin/cos trajectory
|
||||
double max_P = 0.25; // [rad]
|
||||
double max_R = 0.25; // [rad]
|
||||
double t = time_ - duration_;
|
||||
double L_P_des = max_P * std::cos(2.0 * M_PI * t);
|
||||
double L_R_des = max_R * std::sin(2.0 * M_PI * t);
|
||||
double R_P_des = max_P * std::cos(2.0 * M_PI * t);
|
||||
double R_R_des = -max_R * std::sin(2.0 * M_PI * t);
|
||||
```
|
||||
|
||||
**设置踝关节指令**
|
||||
|
||||
```c++
|
||||
// update ankle joint position targets
|
||||
float Kp_Pitch = 80;
|
||||
float Kd_Pitch = 1;
|
||||
float Kp_Roll = 80;
|
||||
float Kd_Roll = 1;
|
||||
dds_low_command.motor_cmd().at(4).q() = L_P_des; // 4: LeftAnklePitch
|
||||
dds_low_command.motor_cmd().at(4).dq() = 0;
|
||||
dds_low_command.motor_cmd().at(4).kp() = Kp_Pitch;
|
||||
dds_low_command.motor_cmd().at(4).kd() = Kd_Pitch;
|
||||
dds_low_command.motor_cmd().at(4).tau() = 0;
|
||||
dds_low_command.motor_cmd().at(5).q() = L_R_des; // 5: LeftAnkleRoll
|
||||
dds_low_command.motor_cmd().at(5).dq() = 0;
|
||||
dds_low_command.motor_cmd().at(5).kp() = Kp_Roll;
|
||||
dds_low_command.motor_cmd().at(5).kd() = Kd_Roll;
|
||||
dds_low_command.motor_cmd().at(5).tau() = 0;
|
||||
dds_low_command.motor_cmd().at(10).q() = R_P_des; // 10: RightAnklePitch
|
||||
dds_low_command.motor_cmd().at(10).dq() = 0;
|
||||
dds_low_command.motor_cmd().at(10).kp() = Kp_Pitch;
|
||||
dds_low_command.motor_cmd().at(10).kd() = Kd_Pitch;
|
||||
dds_low_command.motor_cmd().at(10).tau() = 0;
|
||||
dds_low_command.motor_cmd().at(11).q() = R_R_des; // 11: RightAnkleRoll
|
||||
dds_low_command.motor_cmd().at(11).dq() = 0;
|
||||
dds_low_command.motor_cmd().at(11).kp() = Kp_Roll;
|
||||
dds_low_command.motor_cmd().at(11).kd() = Kd_Roll;
|
||||
dds_low_command.motor_cmd().at(11).tau() = 0;
|
||||
```
|
||||
|
||||
**打印期望值和测量值到终端**
|
||||
|
||||
```c++
|
||||
float L_P_m = low_state_.motor_state().at(4).q();
|
||||
float L_R_m = low_state_.motor_state().at(5).q();
|
||||
float R_P_m = low_state_.motor_state().at(10).q();
|
||||
float R_R_m = low_state_.motor_state().at(11).q();
|
||||
printf("%f,%f,%f,%f,%f,%f,%f,%f\n", L_P_des, L_P_m, L_R_des, L_R_m, R_P_des, R_P_m, R_R_des, R_R_m);
|
||||
```
|
||||
|
||||
| L_P_des | L_P_m | L_R_des | L_R_m | R_P_des | R_P_m | R_R_des | R_R_m |
|
||||
| ----------------- | ----------------- | ---------------- | ---------------- | ----------------- | ----------------- | ---------------- | ---------------- |
|
||||
| 左脚 Pitch 期望值 | 左脚 Pitch 测量值 | 左脚 Roll 期望值 | 左脚 Roll 测量值 | 右脚 Pitch 期望值 | 右脚 Pitch 测量值 | 右脚 Roll 期望值 | 右脚 Roll 测量值 |
|
||||
|
||||
安装并编译 [unitree_sdk2](https://github.com/unitreerobotics/unitree_sdk2)，务必先把机器人**悬挂起来**，然后终端运行测试例程：
|
||||
|
||||
```bash
|
||||
h1_2_ankle_track network_interface
|
||||
```
|
||||
|
||||
启动后，机器人会先恢复到零位，然后周期性摆动脚踝，并打印左右踝关节期望值和测量值，其位置跟踪效果如图：
|
||||
|
||||
<p float="middle">
|
||||
<img src="doc/images/tracking.png" width="49%"/>
|
||||
<img src="doc/images/tracking_circle.png" width="49%"/>
|
||||
</p>
|
||||
|
||||
图中符号定义：
|
||||
|
||||
* `L_Pitch_d`：左脚踝 P 关节期望值
|
||||
* `L_Pitch_m`：左脚踝 P 关节测量值
|
||||
* `L_Roll_d`：左脚踝 R 关节期望值
|
||||
* `L_Roll_m`：左脚踝 R 关节测量值
|
||||
|
||||
左图表示串联 Pitch、Roll 关节能较准确地跟踪正弦曲线目标位置指令；右图表示串联关节在相空间中的跟踪效果。
|
||||
BIN
unitree_sdk2/example/h1/doc/images/ankle.png
Normal file
BIN
unitree_sdk2/example/h1/doc/images/ankle.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 239 KiB |
BIN
unitree_sdk2/example/h1/doc/images/tracking.png
Normal file
BIN
unitree_sdk2/example/h1/doc/images/tracking.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 126 KiB |
BIN
unitree_sdk2/example/h1/doc/images/tracking_circle.png
Normal file
BIN
unitree_sdk2/example/h1/doc/images/tracking_circle.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 66 KiB |
259
unitree_sdk2/example/h1/high_level/h1_2_arm_sdk_dds_example.cpp
Normal file
259
unitree_sdk2/example/h1/high_level/h1_2_arm_sdk_dds_example.cpp
Normal file
@@ -0,0 +1,259 @@
|
||||
#include <array>
|
||||
#include <chrono>
|
||||
#include <iostream>
|
||||
#include <thread>
|
||||
|
||||
#include <unitree/idl/hg/LowCmd_.hpp>
|
||||
#include <unitree/idl/hg/LowState_.hpp>
|
||||
#include <unitree/robot/channel/channel_publisher.hpp>
|
||||
#include <unitree/robot/channel/channel_subscriber.hpp>
|
||||
|
||||
static const std::string kTopicArmSDK = "rt/arm_sdk";
|
||||
static const std::string kTopicState = "rt/lowstate";
|
||||
constexpr float kPi = 3.141592654;
|
||||
constexpr float kPi_2 = 1.57079632;
|
||||
|
||||
enum JointIndex {
|
||||
// Left leg
|
||||
kLeftHipYaw = 0,
|
||||
kLeftHipPitch = 1,
|
||||
kLeftHipRoll = 2,
|
||||
kLeftKnee = 3,
|
||||
kLeftAnkle = 4,
|
||||
kLeftAnkleRoll = 5,
|
||||
// Right leg
|
||||
kRightHipYaw = 6,
|
||||
kRightHipPitch = 7,
|
||||
kRightHipRoll = 8,
|
||||
kRightKnee = 9,
|
||||
kRightAnkle = 10,
|
||||
kRightAnkleRoll = 11,
|
||||
|
||||
kWaistYaw = 12,
|
||||
|
||||
// Left arm
|
||||
kLeftShoulderPitch = 13,
|
||||
kLeftShoulderRoll = 14,
|
||||
kLeftShoulderYaw = 15,
|
||||
kLeftElbow = 16,
|
||||
kLeftWristRoll = 17,
|
||||
kLeftWristPitch = 18,
|
||||
kLeftWristYaw = 19,
|
||||
// Right arm
|
||||
kRightShoulderPitch = 20,
|
||||
kRightShoulderRoll = 21,
|
||||
kRightShoulderYaw = 22,
|
||||
kRightElbow = 23,
|
||||
kRightWristRoll = 24,
|
||||
kRightWristPitch = 25,
|
||||
kRightWristYaw = 26,
|
||||
|
||||
kNotUsedJoint = 27,
|
||||
kNotUsedJoint1 = 28,
|
||||
kNotUsedJoint2 = 29,
|
||||
kNotUsedJoint3 = 30,
|
||||
kNotUsedJoint4 = 31,
|
||||
kNotUsedJoint5 = 32,
|
||||
kNotUsedJoint6 = 33,
|
||||
kNotUsedJoint7 = 34
|
||||
};
|
||||
|
||||
int main(int argc, char const *argv[]) {
|
||||
if (argc < 2) {
|
||||
std::cout << "Usage: " << argv[0] << " networkInterface" << std::endl;
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
unitree::robot::ChannelFactory::Instance()->Init(0, argv[1]);
|
||||
|
||||
unitree::robot::ChannelPublisherPtr<unitree_hg::msg::dds_::LowCmd_>
|
||||
arm_sdk_publisher;
|
||||
unitree_hg::msg::dds_::LowCmd_ msg;
|
||||
|
||||
arm_sdk_publisher.reset(
|
||||
new unitree::robot::ChannelPublisher<unitree_hg::msg::dds_::LowCmd_>(
|
||||
kTopicArmSDK));
|
||||
arm_sdk_publisher->InitChannel();
|
||||
|
||||
unitree::robot::ChannelSubscriberPtr<unitree_hg::msg::dds_::LowState_>
|
||||
low_state_subscriber;
|
||||
|
||||
// create subscriber
|
||||
unitree_hg::msg::dds_::LowState_ state_msg;
|
||||
low_state_subscriber.reset(
|
||||
new unitree::robot::ChannelSubscriber<unitree_hg::msg::dds_::LowState_>(
|
||||
kTopicState));
|
||||
low_state_subscriber->InitChannel([&](const void *msg) {
|
||||
auto s = ( const unitree_hg::msg::dds_::LowState_* )msg;
|
||||
memcpy( &state_msg, s, sizeof( unitree_hg::msg::dds_::LowState_ ) );
|
||||
}, 1);
|
||||
|
||||
std::array<JointIndex, 15> arm_joints = {
|
||||
JointIndex::kLeftShoulderPitch, JointIndex::kLeftShoulderRoll,
|
||||
JointIndex::kLeftShoulderYaw, JointIndex::kLeftElbow,
|
||||
JointIndex::kLeftWristRoll, JointIndex::kLeftWristPitch, JointIndex::kLeftWristYaw,
|
||||
JointIndex::kRightShoulderPitch, JointIndex::kRightShoulderRoll,
|
||||
JointIndex::kRightShoulderYaw, JointIndex::kRightElbow,
|
||||
JointIndex::kRightWristRoll, JointIndex::kRightWristPitch, JointIndex::kRightWristYaw,
|
||||
JointIndex::kWaistYaw};
|
||||
|
||||
float weight = 0.f;
|
||||
float weight_rate = 0.2f;
|
||||
|
||||
std::array<float, 15> kp_array = { 120, 120, 80, 50, 50, 50, 50,
|
||||
120, 120, 80, 50, 50, 50, 50,
|
||||
200 };
|
||||
std::array<float, 15> kd_array = { 2.0, 2.0, 1.5, 1.0, 1.0, 1.0, 1.0,
|
||||
2.0, 2.0, 1.5, 1.0, 1.0, 1.0, 1.0,
|
||||
2.0 };
|
||||
float dq = 0.f;
|
||||
float tau_ff = 0.f;
|
||||
|
||||
float control_dt = 0.02f;
|
||||
float max_joint_velocity = 0.5f;
|
||||
|
||||
float delta_weight = weight_rate * control_dt;
|
||||
float max_joint_delta = max_joint_velocity * control_dt;
|
||||
auto sleep_time =
|
||||
std::chrono::milliseconds(static_cast<int>(control_dt / 0.001f));
|
||||
|
||||
// modify the initial position to avoid collision
|
||||
std::array<float, 15> init_pos{0.f, 0.3, 0.f, 0, 0, 0, 0,
|
||||
0.f, -0.3, 0.f, 0, 0, 0, 0,
|
||||
0.f};
|
||||
|
||||
std::array<float, 15> target_pos = {0.f, kPi_2, 0.f, kPi_2, 0, 0, 0,
|
||||
0.f, -kPi_2, 0.f, kPi_2, 0, 0, 0,
|
||||
0.f};
|
||||
|
||||
// wait for init
|
||||
std::cout << "Press ENTER to init arms ...";
|
||||
std::cin.get();
|
||||
|
||||
|
||||
// get current joint position
|
||||
std::array<float, 15> current_jpos{};
|
||||
std::cout<<"Current joint position: ";
|
||||
for (int i = 0; i < arm_joints.size(); ++i) {
|
||||
current_jpos.at(i) = state_msg.motor_state().at(arm_joints.at(i)).q();
|
||||
std::cout << current_jpos.at(i) << " ";
|
||||
}
|
||||
std::cout << std::endl;
|
||||
|
||||
// set init pos
|
||||
std::cout << "Initailizing arms ...";
|
||||
float init_time = 2.0f;
|
||||
int init_time_steps = static_cast<int>(init_time / control_dt);
|
||||
|
||||
for (int i = 0; i < init_time_steps; ++i) {
|
||||
// set weight
|
||||
weight = 1.0;
|
||||
msg.motor_cmd().at(JointIndex::kNotUsedJoint).q(weight);
|
||||
float phase = 1.0 * i / init_time_steps;
|
||||
std::cout << "Phase: " << phase << std::endl;
|
||||
|
||||
// set control joints
|
||||
for (int j = 0; j < init_pos.size(); ++j) {
|
||||
msg.motor_cmd().at(arm_joints.at(j)).q(init_pos.at(j) * phase + current_jpos.at(j) * (1 - phase));
|
||||
msg.motor_cmd().at(arm_joints.at(j)).dq(dq);
|
||||
msg.motor_cmd().at(arm_joints.at(j)).kp(kp_array.at(j));
|
||||
msg.motor_cmd().at(arm_joints.at(j)).kd(kd_array.at(j));
|
||||
msg.motor_cmd().at(arm_joints.at(j)).tau(tau_ff);
|
||||
}
|
||||
|
||||
// send dds msg
|
||||
arm_sdk_publisher->Write(msg);
|
||||
|
||||
// sleep
|
||||
std::this_thread::sleep_for(sleep_time);
|
||||
}
|
||||
|
||||
std::cout << "Done!" << std::endl;
|
||||
|
||||
// wait for control
|
||||
std::cout << "Press ENTER to start arm ctrl ..." << std::endl;
|
||||
std::cin.get();
|
||||
|
||||
// start control
|
||||
std::cout << "Start arm ctrl!" << std::endl;
|
||||
float period = 5.f;
|
||||
int num_time_steps = static_cast<int>(period / control_dt);
|
||||
|
||||
std::array<float, 15> current_jpos_des{0.f, 0.3, 0.f, 0, 0, 0, 0,
|
||||
0.f, -0.3, 0.f, 0, 0, 0, 0,
|
||||
0.f};
|
||||
|
||||
// lift arms up
|
||||
for (int i = 0; i < num_time_steps; ++i) {
|
||||
// update jpos des
|
||||
for (int j = 0; j < init_pos.size(); ++j) {
|
||||
current_jpos_des.at(j) +=
|
||||
std::clamp(target_pos.at(j) - current_jpos_des.at(j),
|
||||
-max_joint_delta, max_joint_delta);
|
||||
}
|
||||
|
||||
// set control joints
|
||||
for (int j = 0; j < init_pos.size(); ++j) {
|
||||
msg.motor_cmd().at(arm_joints.at(j)).q(current_jpos_des.at(j));
|
||||
msg.motor_cmd().at(arm_joints.at(j)).dq(dq);
|
||||
msg.motor_cmd().at(arm_joints.at(j)).kp(kp_array.at(j));
|
||||
msg.motor_cmd().at(arm_joints.at(j)).kd(kd_array.at(j));
|
||||
msg.motor_cmd().at(arm_joints.at(j)).tau(tau_ff);
|
||||
}
|
||||
|
||||
// send dds msg
|
||||
arm_sdk_publisher->Write(msg);
|
||||
|
||||
// sleep
|
||||
std::this_thread::sleep_for(sleep_time);
|
||||
}
|
||||
|
||||
// put arms down
|
||||
for (int i = 0; i < num_time_steps; ++i) {
|
||||
// update jpos des
|
||||
for (int j = 0; j < init_pos.size(); ++j) {
|
||||
current_jpos_des.at(j) +=
|
||||
std::clamp(init_pos.at(j) - current_jpos_des.at(j), -max_joint_delta,
|
||||
max_joint_delta);
|
||||
}
|
||||
|
||||
// set control joints
|
||||
for (int j = 0; j < init_pos.size(); ++j) {
|
||||
msg.motor_cmd().at(arm_joints.at(j)).q(current_jpos_des.at(j));
|
||||
msg.motor_cmd().at(arm_joints.at(j)).dq(dq);
|
||||
msg.motor_cmd().at(arm_joints.at(j)).kp(kp_array.at(j));
|
||||
msg.motor_cmd().at(arm_joints.at(j)).kd(kd_array.at(j));
|
||||
msg.motor_cmd().at(arm_joints.at(j)).tau(tau_ff);
|
||||
}
|
||||
|
||||
// send dds msg
|
||||
arm_sdk_publisher->Write(msg);
|
||||
|
||||
// sleep
|
||||
std::this_thread::sleep_for(sleep_time);
|
||||
}
|
||||
|
||||
// stop control
|
||||
std::cout << "Stoping arm ctrl ...";
|
||||
float stop_time = 2.0f;
|
||||
int stop_time_steps = static_cast<int>(stop_time / control_dt);
|
||||
|
||||
for (int i = 0; i < stop_time_steps; ++i) {
|
||||
// increase weight
|
||||
weight -= delta_weight;
|
||||
weight = std::clamp(weight, 0.f, 1.f);
|
||||
|
||||
// set weight
|
||||
msg.motor_cmd().at(JointIndex::kNotUsedJoint).q(weight);
|
||||
|
||||
// send dds msg
|
||||
arm_sdk_publisher->Write(msg);
|
||||
|
||||
// sleep
|
||||
std::this_thread::sleep_for(sleep_time);
|
||||
}
|
||||
|
||||
std::cout << "Done!" << std::endl;
|
||||
|
||||
return 0;
|
||||
}
|
||||
234
unitree_sdk2/example/h1/high_level/h1_arm_sdk_dds_example.cpp
Normal file
234
unitree_sdk2/example/h1/high_level/h1_arm_sdk_dds_example.cpp
Normal file
@@ -0,0 +1,234 @@
|
||||
#include <array>
|
||||
#include <chrono>
|
||||
#include <iostream>
|
||||
#include <thread>
|
||||
|
||||
#include <unitree/idl/go2/LowCmd_.hpp>
|
||||
#include <unitree/idl/hg/LowState_.hpp>
|
||||
#include <unitree/robot/channel/channel_publisher.hpp>
|
||||
#include <unitree/robot/channel/channel_subscriber.hpp>
|
||||
|
||||
static const std::string kTopicArmSDK = "rt/arm_sdk";
|
||||
static const std::string kTopicState = "rt/lowstate";
|
||||
constexpr float kPi = 3.141592654;
|
||||
constexpr float kPi_2 = 1.57079632;
|
||||
|
||||
enum JointIndex {
|
||||
// Right leg
|
||||
kRightHipYaw = 8,
|
||||
kRightHipRoll = 0,
|
||||
kRightHipPitch = 1,
|
||||
kRightKnee = 2,
|
||||
kRightAnkle = 11,
|
||||
// Left leg
|
||||
kLeftHipYaw = 7,
|
||||
kLeftHipRoll = 3,
|
||||
kLeftHipPitch = 4,
|
||||
kLeftKnee = 5,
|
||||
kLeftAnkle = 10,
|
||||
|
||||
kWaistYaw = 6,
|
||||
|
||||
kNotUsedJoint = 9,
|
||||
|
||||
// Right arm
|
||||
kRightShoulderPitch = 12,
|
||||
kRightShoulderRoll = 13,
|
||||
kRightShoulderYaw = 14,
|
||||
kRightElbow = 15,
|
||||
// Left arm
|
||||
kLeftShoulderPitch = 16,
|
||||
kLeftShoulderRoll = 17,
|
||||
kLeftShoulderYaw = 18,
|
||||
kLeftElbow = 19,
|
||||
|
||||
};
|
||||
|
||||
int main(int argc, char const *argv[]) {
|
||||
if (argc < 2) {
|
||||
std::cout << "Usage: " << argv[0] << " networkInterface" << std::endl;
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
unitree::robot::ChannelFactory::Instance()->Init(0, argv[1]);
|
||||
|
||||
unitree::robot::ChannelPublisherPtr<unitree_go::msg::dds_::LowCmd_>
|
||||
arm_sdk_publisher;
|
||||
unitree_go::msg::dds_::LowCmd_ msg;
|
||||
|
||||
arm_sdk_publisher.reset(
|
||||
new unitree::robot::ChannelPublisher<unitree_go::msg::dds_::LowCmd_>(
|
||||
kTopicArmSDK));
|
||||
arm_sdk_publisher->InitChannel();
|
||||
|
||||
unitree::robot::ChannelSubscriberPtr<unitree_hg::msg::dds_::LowState_>
|
||||
low_state_subscriber;
|
||||
|
||||
// create subscriber
|
||||
unitree_hg::msg::dds_::LowState_ state_msg;
|
||||
low_state_subscriber.reset(
|
||||
new unitree::robot::ChannelSubscriber<unitree_hg::msg::dds_::LowState_>(
|
||||
kTopicState));
|
||||
low_state_subscriber->InitChannel([&](const void *msg) {
|
||||
auto s = ( const unitree_hg::msg::dds_::LowState_* )msg;
|
||||
memcpy( &state_msg, s, sizeof( unitree_hg::msg::dds_::LowState_ ) );
|
||||
}, 1);
|
||||
|
||||
std::array<JointIndex, 9> arm_joints = {
|
||||
JointIndex::kLeftShoulderPitch, JointIndex::kLeftShoulderRoll,
|
||||
JointIndex::kLeftShoulderYaw, JointIndex::kLeftElbow,
|
||||
JointIndex::kRightShoulderPitch, JointIndex::kRightShoulderRoll,
|
||||
JointIndex::kRightShoulderYaw, JointIndex::kRightElbow, JointIndex::kWaistYaw};
|
||||
|
||||
float weight = 0.f;
|
||||
float weight_rate = 0.2f;
|
||||
|
||||
float kp = 60.f;
|
||||
float kd = 1.5f;
|
||||
float dq = 0.f;
|
||||
float tau_ff = 0.f;
|
||||
|
||||
float control_dt = 0.02f;
|
||||
float max_joint_velocity = 0.5f;
|
||||
|
||||
float delta_weight = weight_rate * control_dt;
|
||||
float max_joint_delta = max_joint_velocity * control_dt;
|
||||
auto sleep_time =
|
||||
std::chrono::milliseconds(static_cast<int>(control_dt / 0.001f));
|
||||
|
||||
std::array<float, 9> init_pos{0, 0, 0, 0,
|
||||
0, 0, 0, 0,
|
||||
0};
|
||||
|
||||
std::array<float, 9> target_pos = {0.f, kPi_2, 0.f, kPi_2,
|
||||
0.f, -kPi_2, 0.f, kPi_2,
|
||||
0.f};
|
||||
|
||||
// wait for init
|
||||
std::cout << "Press ENTER to init arms ...";
|
||||
std::cin.get();
|
||||
|
||||
// get current joint position
|
||||
std::array<float, 9> current_jpos{};
|
||||
std::cout<<"Current joint position: ";
|
||||
for (int i = 0; i < arm_joints.size(); ++i) {
|
||||
current_jpos.at(i) = state_msg.motor_state().at(arm_joints.at(i)).q();
|
||||
std::cout << current_jpos.at(i) << " ";
|
||||
}
|
||||
std::cout << std::endl;
|
||||
|
||||
// set init pos
|
||||
std::cout << "Initailizing arms ...";
|
||||
float init_time = 2.0f;
|
||||
int init_time_steps = static_cast<int>(init_time / control_dt);
|
||||
|
||||
for (int i = 0; i < init_time_steps; ++i) {
|
||||
// set weight
|
||||
weight = 1.0;
|
||||
msg.motor_cmd().at(JointIndex::kNotUsedJoint).q(weight);
|
||||
float phase = 1.0 * i / init_time_steps;
|
||||
std::cout << "Phase: " << phase << std::endl;
|
||||
|
||||
// set control joints
|
||||
for (int j = 0; j < init_pos.size(); ++j) {
|
||||
msg.motor_cmd().at(arm_joints.at(j)).q(init_pos.at(j) * phase + current_jpos.at(j) * (1 - phase));
|
||||
msg.motor_cmd().at(arm_joints.at(j)).dq(dq);
|
||||
msg.motor_cmd().at(arm_joints.at(j)).kp(kp);
|
||||
msg.motor_cmd().at(arm_joints.at(j)).kd(kd);
|
||||
msg.motor_cmd().at(arm_joints.at(j)).tau(tau_ff);
|
||||
}
|
||||
|
||||
// send dds msg
|
||||
arm_sdk_publisher->Write(msg);
|
||||
|
||||
// sleep
|
||||
std::this_thread::sleep_for(sleep_time);
|
||||
}
|
||||
|
||||
std::cout << "Done!" << std::endl;
|
||||
|
||||
// wait for control
|
||||
std::cout << "Press ENTER to start arm ctrl ..." << std::endl;
|
||||
std::cin.get();
|
||||
|
||||
// start control
|
||||
std::cout << "Start arm ctrl!" << std::endl;
|
||||
float period = 5.f;
|
||||
int num_time_steps = static_cast<int>(period / control_dt);
|
||||
|
||||
std::array<float, 9> current_jpos_des{};
|
||||
|
||||
// lift arms up
|
||||
for (int i = 0; i < num_time_steps; ++i) {
|
||||
// update jpos des
|
||||
for (int j = 0; j < init_pos.size(); ++j) {
|
||||
current_jpos_des.at(j) +=
|
||||
std::clamp(target_pos.at(j) - current_jpos_des.at(j),
|
||||
-max_joint_delta, max_joint_delta);
|
||||
}
|
||||
|
||||
// set control joints
|
||||
for (int j = 0; j < init_pos.size(); ++j) {
|
||||
msg.motor_cmd().at(arm_joints.at(j)).q(current_jpos_des.at(j));
|
||||
msg.motor_cmd().at(arm_joints.at(j)).dq(dq);
|
||||
msg.motor_cmd().at(arm_joints.at(j)).kp(kp);
|
||||
msg.motor_cmd().at(arm_joints.at(j)).kd(kd);
|
||||
msg.motor_cmd().at(arm_joints.at(j)).tau(tau_ff);
|
||||
}
|
||||
|
||||
// send dds msg
|
||||
arm_sdk_publisher->Write(msg);
|
||||
|
||||
// sleep
|
||||
std::this_thread::sleep_for(sleep_time);
|
||||
}
|
||||
|
||||
// put arms down
|
||||
for (int i = 0; i < num_time_steps; ++i) {
|
||||
// update jpos des
|
||||
for (int j = 0; j < init_pos.size(); ++j) {
|
||||
current_jpos_des.at(j) +=
|
||||
std::clamp(init_pos.at(j) - current_jpos_des.at(j), -max_joint_delta,
|
||||
max_joint_delta);
|
||||
}
|
||||
|
||||
// set control joints
|
||||
for (int j = 0; j < init_pos.size(); ++j) {
|
||||
msg.motor_cmd().at(arm_joints.at(j)).q(current_jpos_des.at(j));
|
||||
msg.motor_cmd().at(arm_joints.at(j)).dq(dq);
|
||||
msg.motor_cmd().at(arm_joints.at(j)).kp(kp);
|
||||
msg.motor_cmd().at(arm_joints.at(j)).kd(kd);
|
||||
msg.motor_cmd().at(arm_joints.at(j)).tau(tau_ff);
|
||||
}
|
||||
|
||||
// send dds msg
|
||||
arm_sdk_publisher->Write(msg);
|
||||
|
||||
// sleep
|
||||
std::this_thread::sleep_for(sleep_time);
|
||||
}
|
||||
|
||||
// stop control
|
||||
std::cout << "Stoping arm ctrl ...";
|
||||
float stop_time = 2.0f;
|
||||
int stop_time_steps = static_cast<int>(stop_time / control_dt);
|
||||
|
||||
for (int i = 0; i < stop_time_steps; ++i) {
|
||||
// increase weight
|
||||
weight -= delta_weight;
|
||||
weight = std::clamp(weight, 0.f, 1.f);
|
||||
|
||||
// set weight
|
||||
msg.motor_cmd().at(JointIndex::kNotUsedJoint).q(weight);
|
||||
|
||||
// send dds msg
|
||||
arm_sdk_publisher->Write(msg);
|
||||
|
||||
// sleep
|
||||
std::this_thread::sleep_for(sleep_time);
|
||||
}
|
||||
|
||||
std::cout << "Done!" << std::endl;
|
||||
|
||||
return 0;
|
||||
}
|
||||
300
unitree_sdk2/example/h1/high_level/h1_loco_client_example.cpp
Normal file
300
unitree_sdk2/example/h1/high_level/h1_loco_client_example.cpp
Normal file
@@ -0,0 +1,300 @@
|
||||
#include <chrono>
|
||||
#include <iostream>
|
||||
#include <thread>
|
||||
|
||||
#include <unitree/robot/h1/loco/h1_loco_api.hpp>
|
||||
#include <unitree/robot/h1/loco/h1_loco_client.hpp>
|
||||
|
||||
std::vector<float> stringToFloatVector(const std::string &str) {
|
||||
std::vector<float> result;
|
||||
std::stringstream ss(str);
|
||||
float num;
|
||||
while (ss >> num) {
|
||||
result.push_back(num);
|
||||
// ignore any trailing whitespace
|
||||
ss.ignore();
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
int main(int argc, char const *argv[]) {
|
||||
std::map<std::string, std::string> args = {{"network_interface", "lo"}};
|
||||
|
||||
std::map<std::string, std::string> values;
|
||||
for (int i = 1; i < argc; ++i) {
|
||||
std::string arg = argv[i];
|
||||
if (arg.substr(0, 2) == "--") {
|
||||
size_t pos = arg.find("=");
|
||||
std::string key, value;
|
||||
if (pos != std::string::npos) {
|
||||
key = arg.substr(2, pos - 2);
|
||||
value = arg.substr(pos + 1);
|
||||
|
||||
if (value.front() == '"' && value.back() == '"') {
|
||||
value = value.substr(1, value.length() - 2);
|
||||
}
|
||||
} else {
|
||||
key = arg.substr(2);
|
||||
value = "";
|
||||
}
|
||||
if (args.find(key) != args.end()) {
|
||||
args[key] = value;
|
||||
} else {
|
||||
args.insert({{key, value}});
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
unitree::robot::ChannelFactory::Instance()->Init(0, args["network_interface"]);
|
||||
|
||||
unitree::robot::h1::LocoClient client;
|
||||
|
||||
client.Init();
|
||||
client.SetTimeout(10.f);
|
||||
|
||||
for (const auto &arg_pair : args) {
|
||||
std::cout << "Processing command: [" << arg_pair.first << "] with param: [" << arg_pair.second << "] ..."
|
||||
<< std::endl;
|
||||
if (arg_pair.first == "network_interface") {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (arg_pair.first == "get_fsm_id") {
|
||||
int fsm_id;
|
||||
client.GetFsmId(fsm_id);
|
||||
std::cout << "current fsm_id: " << fsm_id << std::endl;
|
||||
}
|
||||
|
||||
if (arg_pair.first == "get_fsm_mode") {
|
||||
int fsm_mode;
|
||||
client.GetFsmMode(fsm_mode);
|
||||
std::cout << "current fsm_mode: " << fsm_mode << std::endl;
|
||||
}
|
||||
|
||||
if (arg_pair.first == "get_balance_mode") {
|
||||
int balance_mode;
|
||||
client.GetBalanceMode(balance_mode);
|
||||
std::cout << "current balance_mode: " << balance_mode << std::endl;
|
||||
}
|
||||
|
||||
if (arg_pair.first == "get_swing_height") {
|
||||
float swing_height;
|
||||
client.GetSwingHeight(swing_height);
|
||||
std::cout << "current swing_height: " << swing_height << std::endl;
|
||||
}
|
||||
|
||||
if (arg_pair.first == "get_stand_height") {
|
||||
float stand_height;
|
||||
client.GetStandHeight(stand_height);
|
||||
std::cout << "current stand_height: " << stand_height << std::endl;
|
||||
}
|
||||
|
||||
if (arg_pair.first == "get_phase") {
|
||||
std::vector<float> phase;
|
||||
client.GetPhase(phase);
|
||||
std::cout << "current phase: (";
|
||||
for (const auto &p : phase) {
|
||||
std::cout << p << ", ";
|
||||
}
|
||||
std::cout << ")" << std::endl;
|
||||
}
|
||||
|
||||
if (arg_pair.first == "enable_odom") {
|
||||
float x, y, yaw;
|
||||
client.EnableOdom();
|
||||
std::cout << "Send enable odom signal" << std::endl;
|
||||
}
|
||||
|
||||
if (arg_pair.first == "disable_odom") {
|
||||
float x, y, yaw;
|
||||
client.DisableOdom();
|
||||
std::cout << "Send disable odom signal" << std::endl;
|
||||
}
|
||||
|
||||
if (arg_pair.first == "get_odom") {
|
||||
float x, y, yaw;
|
||||
client.GetOdom(x, y, yaw);
|
||||
std::cout << "Get Odom: (" << x << ", " << y << ", " << yaw << ")" << std::endl;
|
||||
}
|
||||
|
||||
if (arg_pair.first == "set_fsm_id") {
|
||||
int fsm_id = std::stoi(arg_pair.second);
|
||||
client.SetFsmId(fsm_id);
|
||||
std::cout << "set fsm_id to " << fsm_id << std::endl;
|
||||
}
|
||||
|
||||
if (arg_pair.first == "set_balance_mode") {
|
||||
int balance_mode = std::stoi(arg_pair.second);
|
||||
client.SetBalanceMode(balance_mode);
|
||||
std::cout << "set balance_mode to " << balance_mode << std::endl;
|
||||
}
|
||||
|
||||
if (arg_pair.first == "set_swing_height") {
|
||||
float swing_height = std::stof(arg_pair.second);
|
||||
client.SetSwingHeight(swing_height);
|
||||
std::cout << "set swing_height to " << swing_height << std::endl;
|
||||
}
|
||||
|
||||
if (arg_pair.first == "set_stand_height") {
|
||||
float stand_height = std::stof(arg_pair.second);
|
||||
client.SetStandHeight(stand_height);
|
||||
std::cout << "set stand_height to " << stand_height << std::endl;
|
||||
}
|
||||
|
||||
if (arg_pair.first == "set_velocity") {
|
||||
std::vector<float> param = stringToFloatVector(arg_pair.second);
|
||||
auto param_size = param.size();
|
||||
float vx, vy, omega, duration;
|
||||
if (param_size == 3) {
|
||||
vx = param.at(0);
|
||||
vy = param.at(1);
|
||||
omega = param.at(2);
|
||||
duration = 1.f;
|
||||
} else if (param_size == 4) {
|
||||
vx = param.at(0);
|
||||
vy = param.at(1);
|
||||
omega = param.at(2);
|
||||
duration = param.at(3);
|
||||
} else {
|
||||
std::cerr << "Invalid param size for method SetVelocity: " << param_size << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
client.SetVelocity(vx, vy, omega, duration);
|
||||
std::cout << "set velocity to " << arg_pair.second << std::endl;
|
||||
}
|
||||
|
||||
if (arg_pair.first == "set_phase") {
|
||||
std::vector<float> param = stringToFloatVector(arg_pair.second);
|
||||
auto param_size = param.size();
|
||||
if (param_size == 2) {
|
||||
client.SetPhase(param);
|
||||
std::cout << "set phase to " << arg_pair.second << std::endl;
|
||||
} else {
|
||||
std::cerr << "Invalid param size for method SetPhase: " << param_size << std::endl;
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
if (arg_pair.first == "set_target_pos") {
|
||||
std::vector<float> param = stringToFloatVector(arg_pair.second);
|
||||
client.SetTargetPos(param.at(0), param.at(1), param.at(2), false);
|
||||
std::cout << "set_target_pos: " << arg_pair.second << std::endl;
|
||||
}
|
||||
|
||||
if (arg_pair.first == "set_target_pos_relative") {
|
||||
std::vector<float> param = stringToFloatVector(arg_pair.second);
|
||||
client.SetTargetPos(param.at(0), param.at(1), param.at(2));
|
||||
std::cout << "set_target_pos: " << arg_pair.second << std::endl;
|
||||
}
|
||||
|
||||
if (arg_pair.first == "damp") {
|
||||
client.Damp();
|
||||
}
|
||||
|
||||
if (arg_pair.first == "start") {
|
||||
client.Start();
|
||||
}
|
||||
|
||||
if (arg_pair.first == "stand_up") {
|
||||
client.StandUp();
|
||||
}
|
||||
|
||||
if (arg_pair.first == "zero_torque") {
|
||||
client.ZeroTorque();
|
||||
}
|
||||
|
||||
if (arg_pair.first == "stop_move") {
|
||||
client.StopMove();
|
||||
}
|
||||
|
||||
if (arg_pair.first == "high_stand") {
|
||||
client.HighStand();
|
||||
}
|
||||
|
||||
if (arg_pair.first == "low_stand") {
|
||||
client.LowStand();
|
||||
}
|
||||
|
||||
if (arg_pair.first == "balance_stand") {
|
||||
client.BalanceStand();
|
||||
}
|
||||
|
||||
if (arg_pair.first == "continous_gait") {
|
||||
bool flag;
|
||||
if (arg_pair.second == "true") {
|
||||
flag = true;
|
||||
} else if (arg_pair.second == "false") {
|
||||
flag = false;
|
||||
} else {
|
||||
std::cerr << "invalid argument: " << arg_pair.second << std::endl;
|
||||
return 1;
|
||||
}
|
||||
client.ContinuousGait(flag);
|
||||
}
|
||||
|
||||
if (arg_pair.first == "switch_move_mode") {
|
||||
bool flag;
|
||||
if (arg_pair.second == "true") {
|
||||
flag = true;
|
||||
} else if (arg_pair.second == "false") {
|
||||
flag = false;
|
||||
} else {
|
||||
std::cerr << "invalid argument: " << arg_pair.second << std::endl;
|
||||
return 1;
|
||||
}
|
||||
client.SwitchMoveMode(flag);
|
||||
}
|
||||
|
||||
if (arg_pair.first == "move") {
|
||||
std::vector<float> param = stringToFloatVector(arg_pair.second);
|
||||
auto param_size = param.size();
|
||||
float vx, vy, omega;
|
||||
if (param_size == 3) {
|
||||
vx = param.at(0);
|
||||
vy = param.at(1);
|
||||
omega = param.at(2);
|
||||
} else {
|
||||
std::cerr << "Invalid param size for method SetVelocity: " << param_size << std::endl;
|
||||
return 1;
|
||||
}
|
||||
client.Move(vx, vy, omega);
|
||||
}
|
||||
|
||||
if (arg_pair.first == "set_next_foot") {
|
||||
bool flag;
|
||||
if (arg_pair.second == "0") {
|
||||
flag = true;
|
||||
} else if (arg_pair.second == "1") {
|
||||
flag = false;
|
||||
} else {
|
||||
std::cerr << "invalid argument: " << arg_pair.second << std::endl;
|
||||
return 1;
|
||||
}
|
||||
client.SetNextFoot(flag);
|
||||
}
|
||||
|
||||
if (arg_pair.first == "set_task_id") {
|
||||
int task_id = std::stoi(arg_pair.second);
|
||||
client.SetTaskId(task_id);
|
||||
std::cout << "set task_id to " << task_id << std::endl;
|
||||
}
|
||||
|
||||
if (arg_pair.first == "shake_hand") {
|
||||
client.ShakeHand(0);
|
||||
std::cout << "Shake hand starts! Waiting for 10 s for ending" << std::endl;
|
||||
std::this_thread::sleep_for(std::chrono::seconds(10));
|
||||
std::cout << "Shake hand ends!" << std::endl;
|
||||
client.ShakeHand(1);
|
||||
}
|
||||
|
||||
if (arg_pair.first == "wave_hand") {
|
||||
client.WaveHand();
|
||||
std::cout << "wave hand" << std::endl;
|
||||
}
|
||||
|
||||
std::cout << "Done!" << std::endl;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
8
unitree_sdk2/example/h1/low_level/base_state.h
Normal file
8
unitree_sdk2/example/h1/low_level/base_state.h
Normal file
@@ -0,0 +1,8 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
|
||||
struct BaseState {
|
||||
std::array<float, 3> rpy = {};
|
||||
std::array<float, 3> omega = {};
|
||||
};
|
||||
28
unitree_sdk2/example/h1/low_level/data_buffer.hpp
Normal file
28
unitree_sdk2/example/h1/low_level/data_buffer.hpp
Normal file
@@ -0,0 +1,28 @@
|
||||
#pragma once
|
||||
|
||||
#include <deque>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <shared_mutex>
|
||||
|
||||
template <typename T> class DataBuffer {
|
||||
public:
|
||||
void SetData(const T &newData) {
|
||||
std::unique_lock<std::shared_mutex> lock(mutex);
|
||||
data = std::make_shared<T>(newData);
|
||||
}
|
||||
|
||||
std::shared_ptr<const T> GetData() {
|
||||
std::shared_lock<std::shared_mutex> lock(mutex);
|
||||
return data ? data : nullptr;
|
||||
}
|
||||
|
||||
void Clear() {
|
||||
std::unique_lock<std::shared_mutex> lock(mutex);
|
||||
data = nullptr;
|
||||
}
|
||||
|
||||
private:
|
||||
std::shared_ptr<T> data;
|
||||
std::shared_mutex mutex;
|
||||
};
|
||||
367
unitree_sdk2/example/h1/low_level/h1_27dof_example.cpp
Normal file
367
unitree_sdk2/example/h1/low_level/h1_27dof_example.cpp
Normal file
@@ -0,0 +1,367 @@
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <shared_mutex>
|
||||
|
||||
// DDS
|
||||
#include <unitree/robot/channel/channel_publisher.hpp>
|
||||
#include <unitree/robot/channel/channel_subscriber.hpp>
|
||||
|
||||
// IDL
|
||||
#include <unitree/idl/hg/LowCmd_.hpp>
|
||||
#include <unitree/idl/hg/LowState_.hpp>
|
||||
|
||||
static const std::string HG_CMD_TOPIC = "rt/lowcmd";
|
||||
static const std::string HG_STATE_TOPIC = "rt/lowstate";
|
||||
|
||||
using namespace unitree::common;
|
||||
using namespace unitree::robot;
|
||||
|
||||
const int H1_NUM_MOTOR = 27;
|
||||
|
||||
template <typename T>
|
||||
class DataBuffer {
|
||||
public:
|
||||
void SetData(const T &newData) {
|
||||
std::unique_lock<std::shared_mutex> lock(mutex);
|
||||
data = std::make_shared<T>(newData);
|
||||
}
|
||||
|
||||
std::shared_ptr<const T> GetData() {
|
||||
std::shared_lock<std::shared_mutex> lock(mutex);
|
||||
return data ? data : nullptr;
|
||||
}
|
||||
|
||||
void Clear() {
|
||||
std::unique_lock<std::shared_mutex> lock(mutex);
|
||||
data = nullptr;
|
||||
}
|
||||
|
||||
private:
|
||||
std::shared_ptr<T> data;
|
||||
std::shared_mutex mutex;
|
||||
};
|
||||
|
||||
struct ImuState {
|
||||
std::array<float, 3> rpy = {};
|
||||
std::array<float, 3> omega = {};
|
||||
};
|
||||
|
||||
struct MotorCommand {
|
||||
std::array<float, H1_NUM_MOTOR> q_target = {};
|
||||
std::array<float, H1_NUM_MOTOR> dq_target = {};
|
||||
std::array<float, H1_NUM_MOTOR> kp = {};
|
||||
std::array<float, H1_NUM_MOTOR> kd = {};
|
||||
std::array<float, H1_NUM_MOTOR> tau_ff = {};
|
||||
};
|
||||
|
||||
struct MotorState {
|
||||
std::array<float, H1_NUM_MOTOR> q = {};
|
||||
std::array<float, H1_NUM_MOTOR> dq = {};
|
||||
};
|
||||
|
||||
enum MotorType { GearboxS = 0, GearboxM = 1, GearboxL = 2 };
|
||||
|
||||
std::array<MotorType, H1_NUM_MOTOR> H1MotorType{
|
||||
// clang-format off
|
||||
// legs
|
||||
GearboxM, GearboxM, GearboxM, GearboxL, GearboxS, GearboxS,
|
||||
GearboxM, GearboxM, GearboxM, GearboxL, GearboxS, GearboxS,
|
||||
// waist
|
||||
GearboxM,
|
||||
// arms
|
||||
GearboxS, GearboxS, GearboxS, GearboxS, GearboxS, GearboxS, GearboxS,
|
||||
GearboxS, GearboxS, GearboxS, GearboxS, GearboxS, GearboxS, GearboxS
|
||||
// clang-format on
|
||||
};
|
||||
|
||||
enum PRorAB { PR = 0, AB = 1 };
|
||||
|
||||
enum H1JointIndex {
|
||||
// legs
|
||||
LeftHipYaw = 0,
|
||||
LeftHipPitch = 1,
|
||||
LeftHipRoll = 2,
|
||||
LeftKnee = 3,
|
||||
LeftAnklePitch = 4,
|
||||
LeftAnkleB = 4,
|
||||
LeftAnkleRoll = 5,
|
||||
LeftAnkleA = 5,
|
||||
RightHipYaw = 6,
|
||||
RightHipPitch = 7,
|
||||
RightHipRoll = 8,
|
||||
RightKnee = 9,
|
||||
RightAnklePitch = 10,
|
||||
RightAnkleB = 10,
|
||||
RightAnkleRoll = 11,
|
||||
RightAnkleA = 11,
|
||||
// torso
|
||||
WaistYaw = 12,
|
||||
// arms
|
||||
LeftShoulderPitch = 13,
|
||||
LeftShoulderRoll = 14,
|
||||
LeftShoulderYaw = 15,
|
||||
LeftElbow = 16,
|
||||
LeftWristRoll = 17,
|
||||
LeftWristPitch = 18,
|
||||
LeftWristYaw = 19,
|
||||
RightShoulderPitch = 20,
|
||||
RightShoulderRoll = 21,
|
||||
RightShoulderYaw = 22,
|
||||
RightElbow = 23,
|
||||
RightWristRoll = 24,
|
||||
RightWristPitch = 25,
|
||||
RightWristYaw = 26
|
||||
};
|
||||
|
||||
inline uint32_t Crc32Core(uint32_t *ptr, uint32_t len) {
|
||||
uint32_t xbit = 0;
|
||||
uint32_t data = 0;
|
||||
uint32_t CRC32 = 0xFFFFFFFF;
|
||||
const uint32_t dwPolynomial = 0x04c11db7;
|
||||
for (uint32_t i = 0; i < len; i++) {
|
||||
xbit = 1 << 31;
|
||||
data = ptr[i];
|
||||
for (uint32_t bits = 0; bits < 32; bits++) {
|
||||
if (CRC32 & 0x80000000) {
|
||||
CRC32 <<= 1;
|
||||
CRC32 ^= dwPolynomial;
|
||||
} else
|
||||
CRC32 <<= 1;
|
||||
if (data & xbit) CRC32 ^= dwPolynomial;
|
||||
|
||||
xbit >>= 1;
|
||||
}
|
||||
}
|
||||
return CRC32;
|
||||
};
|
||||
|
||||
float GetMotorKp(MotorType type) {
|
||||
switch (type) {
|
||||
case GearboxS:
|
||||
return 80;
|
||||
case GearboxM:
|
||||
return 100;
|
||||
case GearboxL:
|
||||
return 200;
|
||||
default:
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
float GetMotorKd(MotorType type) {
|
||||
switch (type) {
|
||||
case GearboxS:
|
||||
return 2;
|
||||
case GearboxM:
|
||||
return 3;
|
||||
case GearboxL:
|
||||
return 5;
|
||||
default:
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
class H1Example {
|
||||
private:
|
||||
double time_;
|
||||
double control_dt_; // [2ms]
|
||||
double duration_; // [3 s]
|
||||
PRorAB mode_;
|
||||
uint8_t mode_machine_;
|
||||
|
||||
DataBuffer<MotorState> motor_state_buffer_;
|
||||
DataBuffer<MotorCommand> motor_command_buffer_;
|
||||
DataBuffer<ImuState> imu_state_buffer_;
|
||||
|
||||
ChannelPublisherPtr<unitree_hg::msg::dds_::LowCmd_> lowcmd_publisher_;
|
||||
ChannelSubscriberPtr<unitree_hg::msg::dds_::LowState_> lowstate_subscriber_;
|
||||
ThreadPtr command_writer_ptr_, control_thread_ptr_;
|
||||
|
||||
public:
|
||||
H1Example(std::string networkInterface)
|
||||
: time_(0.0),
|
||||
control_dt_(0.002),
|
||||
duration_(3.0),
|
||||
mode_(PR),
|
||||
mode_machine_(0) {
|
||||
ChannelFactory::Instance()->Init(0, networkInterface);
|
||||
|
||||
// create publisher
|
||||
lowcmd_publisher_.reset(
|
||||
new ChannelPublisher<unitree_hg::msg::dds_::LowCmd_>(HG_CMD_TOPIC));
|
||||
lowcmd_publisher_->InitChannel();
|
||||
|
||||
// create subscriber
|
||||
lowstate_subscriber_.reset(
|
||||
new ChannelSubscriber<unitree_hg::msg::dds_::LowState_>(
|
||||
HG_STATE_TOPIC));
|
||||
lowstate_subscriber_->InitChannel(
|
||||
std::bind(&H1Example::LowStateHandler, this, std::placeholders::_1), 1);
|
||||
|
||||
// create threads
|
||||
command_writer_ptr_ =
|
||||
CreateRecurrentThreadEx("command_writer", UT_CPU_ID_NONE, 2000,
|
||||
&H1Example::LowCommandWriter, this);
|
||||
control_thread_ptr_ = CreateRecurrentThreadEx(
|
||||
"control", UT_CPU_ID_NONE, 2000, &H1Example::Control, this);
|
||||
}
|
||||
|
||||
void ReportRPY() {
|
||||
const std::shared_ptr<const ImuState> imu_tmp_ptr =
|
||||
imu_state_buffer_.GetData();
|
||||
if (imu_tmp_ptr) {
|
||||
std::cout << "rpy: [" << imu_tmp_ptr->rpy.at(0) << ", "
|
||||
<< imu_tmp_ptr->rpy.at(1) << ", " << imu_tmp_ptr->rpy.at(2)
|
||||
<< "]" << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
void LowStateHandler(const void *message) {
|
||||
unitree_hg::msg::dds_::LowState_ low_state =
|
||||
*(const unitree_hg::msg::dds_::LowState_ *)message;
|
||||
|
||||
if (low_state.crc() !=
|
||||
Crc32Core((uint32_t *)&low_state,
|
||||
(sizeof(unitree_hg::msg::dds_::LowState_) >> 2) - 1)) {
|
||||
std::cout << "low_state CRC Error" << std::endl;
|
||||
return;
|
||||
}
|
||||
|
||||
// get motor state
|
||||
MotorState ms_tmp;
|
||||
for (int i = 0; i < H1_NUM_MOTOR; ++i) {
|
||||
ms_tmp.q.at(i) = low_state.motor_state()[i].q();
|
||||
ms_tmp.dq.at(i) = low_state.motor_state()[i].dq();
|
||||
|
||||
if (low_state.motor_state()[i].motorstate())
|
||||
std::cout << "[ERROR] motor " << i << " with code "
|
||||
<< low_state.motor_state()[i].motorstate() << "\n";
|
||||
}
|
||||
motor_state_buffer_.SetData(ms_tmp);
|
||||
|
||||
// get imu state
|
||||
ImuState imu_tmp;
|
||||
imu_tmp.omega = low_state.imu_state().gyroscope();
|
||||
imu_tmp.rpy = low_state.imu_state().rpy();
|
||||
imu_state_buffer_.SetData(imu_tmp);
|
||||
|
||||
// update mode machine
|
||||
if (mode_machine_ != low_state.mode_machine()) {
|
||||
if (mode_machine_ == 0)
|
||||
std::cout << "G1 type: " << unsigned(low_state.mode_machine())
|
||||
<< std::endl;
|
||||
mode_machine_ = low_state.mode_machine();
|
||||
}
|
||||
}
|
||||
|
||||
void LowCommandWriter() {
|
||||
unitree_hg::msg::dds_::LowCmd_ dds_low_command;
|
||||
dds_low_command.mode_pr() = mode_;
|
||||
dds_low_command.mode_machine() = mode_machine_;
|
||||
|
||||
const std::shared_ptr<const MotorCommand> mc =
|
||||
motor_command_buffer_.GetData();
|
||||
if (mc) {
|
||||
for (size_t i = 0; i < H1_NUM_MOTOR; i++) {
|
||||
dds_low_command.motor_cmd().at(i).mode() = 1; // 1:Enable, 0:Disable
|
||||
dds_low_command.motor_cmd().at(i).tau() = mc->tau_ff.at(i);
|
||||
dds_low_command.motor_cmd().at(i).q() = mc->q_target.at(i);
|
||||
dds_low_command.motor_cmd().at(i).dq() = mc->dq_target.at(i);
|
||||
dds_low_command.motor_cmd().at(i).kp() = mc->kp.at(i);
|
||||
dds_low_command.motor_cmd().at(i).kd() = mc->kd.at(i);
|
||||
}
|
||||
|
||||
dds_low_command.crc() = Crc32Core((uint32_t *)&dds_low_command,
|
||||
(sizeof(dds_low_command) >> 2) - 1);
|
||||
lowcmd_publisher_->Write(dds_low_command);
|
||||
}
|
||||
}
|
||||
|
||||
void Control() {
|
||||
ReportRPY();
|
||||
|
||||
MotorCommand motor_command_tmp;
|
||||
const std::shared_ptr<const MotorState> ms = motor_state_buffer_.GetData();
|
||||
|
||||
if (ms) {
|
||||
time_ += control_dt_;
|
||||
if (time_ < duration_ * 1) {
|
||||
// [Stage 1]: set robot to zero posture
|
||||
for (int i = 0; i < H1_NUM_MOTOR; ++i) {
|
||||
double ratio = std::clamp(time_ / duration_, 0.0, 1.0);
|
||||
|
||||
double q_des = 0;
|
||||
motor_command_tmp.tau_ff.at(i) = 0.0;
|
||||
motor_command_tmp.q_target.at(i) =
|
||||
(q_des - ms->q.at(i)) * ratio + ms->q.at(i);
|
||||
motor_command_tmp.dq_target.at(i) = 0.0;
|
||||
motor_command_tmp.kp.at(i) = GetMotorKp(H1MotorType[i]);
|
||||
motor_command_tmp.kd.at(i) = GetMotorKd(H1MotorType[i]);
|
||||
}
|
||||
} else if (time_ < duration_ * 2) {
|
||||
// [Stage 2]: swing ankle's PR
|
||||
mode_ = PR;
|
||||
double max_P = M_PI * 30.0 / 180.0;
|
||||
double max_R = M_PI * 10.0 / 180.0;
|
||||
double t = time_ - duration_ * 1;
|
||||
double L_P_des = +max_P * std::sin(2.0 * M_PI * t);
|
||||
double L_R_des = +max_R * std::sin(2.0 * M_PI * t);
|
||||
double R_P_des = +max_P * std::sin(2.0 * M_PI * t);
|
||||
double R_R_des = -max_R * std::sin(2.0 * M_PI * t);
|
||||
|
||||
for (int i = 0; i < H1_NUM_MOTOR; ++i) {
|
||||
motor_command_tmp.tau_ff.at(i) = 0.0;
|
||||
motor_command_tmp.q_target.at(i) = 0.0;
|
||||
motor_command_tmp.dq_target.at(i) = 0.0;
|
||||
motor_command_tmp.kp.at(i) = GetMotorKp(H1MotorType[i]);
|
||||
motor_command_tmp.kd.at(i) = GetMotorKd(H1MotorType[i]);
|
||||
}
|
||||
|
||||
motor_command_tmp.q_target.at(LeftAnklePitch) = L_P_des;
|
||||
motor_command_tmp.q_target.at(LeftAnkleRoll) = L_R_des;
|
||||
motor_command_tmp.q_target.at(RightAnklePitch) = R_P_des;
|
||||
motor_command_tmp.q_target.at(RightAnkleRoll) = R_R_des;
|
||||
} else {
|
||||
// [Stage 3]: swing ankle's AB
|
||||
mode_ = AB;
|
||||
double max_A = M_PI * 30.0 / 180.0;
|
||||
double max_B = M_PI * 10.0 / 180.0;
|
||||
double t = time_ - duration_ * 2;
|
||||
double L_A_des = +max_A * std::sin(M_PI * t);
|
||||
double L_B_des = +max_B * std::sin(M_PI * t + M_PI);
|
||||
double R_A_des = -max_A * std::sin(M_PI * t);
|
||||
double R_B_des = -max_B * std::sin(M_PI * t + M_PI);
|
||||
|
||||
for (int i = 0; i < H1_NUM_MOTOR; ++i) {
|
||||
motor_command_tmp.tau_ff.at(i) = 0.0;
|
||||
motor_command_tmp.q_target.at(i) = 0.0;
|
||||
motor_command_tmp.dq_target.at(i) = 0.0;
|
||||
motor_command_tmp.kp.at(i) = GetMotorKp(H1MotorType[i]);
|
||||
motor_command_tmp.kd.at(i) = GetMotorKd(H1MotorType[i]);
|
||||
}
|
||||
|
||||
motor_command_tmp.q_target.at(LeftAnkleA) = L_A_des;
|
||||
motor_command_tmp.q_target.at(LeftAnkleB) = L_B_des;
|
||||
motor_command_tmp.q_target.at(RightAnkleA) = R_A_des;
|
||||
motor_command_tmp.q_target.at(RightAnkleB) = R_B_des;
|
||||
}
|
||||
|
||||
motor_command_buffer_.SetData(motor_command_tmp);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
int main(int argc, char const *argv[]) {
|
||||
if (argc < 2) {
|
||||
std::cout << "Usage: h1_27dof_example network_interface_name" << std::endl;
|
||||
exit(0);
|
||||
}
|
||||
std::string networkInterface = argv[1];
|
||||
H1Example custom(networkInterface);
|
||||
|
||||
while (true) sleep(10);
|
||||
|
||||
return 0;
|
||||
}
|
||||
175
unitree_sdk2/example/h1/low_level/h1_2_ankle_track.cpp
Normal file
175
unitree_sdk2/example/h1/low_level/h1_2_ankle_track.cpp
Normal file
@@ -0,0 +1,175 @@
|
||||
#include <cmath>
|
||||
|
||||
// DDS
|
||||
#include <unitree/robot/channel/channel_publisher.hpp>
|
||||
#include <unitree/robot/channel/channel_subscriber.hpp>
|
||||
|
||||
// IDL
|
||||
#include <unitree/idl/hg/LowCmd_.hpp>
|
||||
#include <unitree/idl/hg/LowState_.hpp>
|
||||
|
||||
static const std::string HG_CMD_TOPIC = "rt/lowcmd";
|
||||
static const std::string HG_STATE_TOPIC = "rt/lowstate";
|
||||
|
||||
using namespace unitree::common;
|
||||
using namespace unitree::robot;
|
||||
|
||||
const int H1_NUM_MOTOR = 27;
|
||||
|
||||
enum PRorAB { PR = 0, AB = 1 };
|
||||
|
||||
inline uint32_t Crc32Core(uint32_t *ptr, uint32_t len) {
|
||||
uint32_t xbit = 0;
|
||||
uint32_t data = 0;
|
||||
uint32_t CRC32 = 0xFFFFFFFF;
|
||||
const uint32_t dwPolynomial = 0x04c11db7;
|
||||
for (uint32_t i = 0; i < len; i++) {
|
||||
xbit = 1 << 31;
|
||||
data = ptr[i];
|
||||
for (uint32_t bits = 0; bits < 32; bits++) {
|
||||
if (CRC32 & 0x80000000) {
|
||||
CRC32 <<= 1;
|
||||
CRC32 ^= dwPolynomial;
|
||||
} else
|
||||
CRC32 <<= 1;
|
||||
if (data & xbit) CRC32 ^= dwPolynomial;
|
||||
|
||||
xbit >>= 1;
|
||||
}
|
||||
}
|
||||
return CRC32;
|
||||
};
|
||||
|
||||
class H1Example {
|
||||
private:
|
||||
double time_;
|
||||
double control_dt_; // [2ms]
|
||||
double duration_; // [3 s]
|
||||
PRorAB mode_;
|
||||
|
||||
unitree_hg::msg::dds_::LowState_ low_state_;
|
||||
|
||||
ChannelPublisherPtr<unitree_hg::msg::dds_::LowCmd_> lowcmd_publisher_;
|
||||
ChannelSubscriberPtr<unitree_hg::msg::dds_::LowState_> lowstate_subscriber_;
|
||||
ThreadPtr command_writer_ptr_, control_thread_ptr_;
|
||||
|
||||
public:
|
||||
H1Example(std::string networkInterface)
|
||||
: time_(0.0), control_dt_(0.002), duration_(3.0), mode_(PR) {
|
||||
ChannelFactory::Instance()->Init(0, networkInterface);
|
||||
|
||||
// create publisher
|
||||
lowcmd_publisher_.reset(
|
||||
new ChannelPublisher<unitree_hg::msg::dds_::LowCmd_>(HG_CMD_TOPIC));
|
||||
lowcmd_publisher_->InitChannel();
|
||||
|
||||
// create subscriber
|
||||
lowstate_subscriber_.reset(
|
||||
new ChannelSubscriber<unitree_hg::msg::dds_::LowState_>(
|
||||
HG_STATE_TOPIC));
|
||||
lowstate_subscriber_->InitChannel(
|
||||
std::bind(&H1Example::LowStateHandler, this, std::placeholders::_1), 1);
|
||||
|
||||
// create threads
|
||||
control_thread_ptr_ = CreateRecurrentThreadEx(
|
||||
"control", UT_CPU_ID_NONE, 2000, &H1Example::Control, this);
|
||||
}
|
||||
|
||||
void LowStateHandler(const void *message) {
|
||||
low_state_ = *(const unitree_hg::msg::dds_::LowState_ *)message;
|
||||
|
||||
if (low_state_.crc() !=
|
||||
Crc32Core((uint32_t *)&low_state_,
|
||||
(sizeof(unitree_hg::msg::dds_::LowState_) >> 2) - 1)) {
|
||||
std::cout << "low_state CRC Error" << std::endl;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
void Control() {
|
||||
unitree_hg::msg::dds_::LowCmd_ dds_low_command;
|
||||
dds_low_command.mode_pr() = mode_;
|
||||
dds_low_command.mode_machine() = low_state_.mode_machine();
|
||||
for (int i = 0; i < H1_NUM_MOTOR; ++i) {
|
||||
dds_low_command.motor_cmd().at(i).mode() = 1; // 1:Enable, 0:Disable
|
||||
dds_low_command.motor_cmd().at(i).tau() = 0.0;
|
||||
dds_low_command.motor_cmd().at(i).q() = 0.0;
|
||||
dds_low_command.motor_cmd().at(i).dq() = 0.0;
|
||||
dds_low_command.motor_cmd().at(i).kp() = (i < 13) ? 100.0 : 50.0;
|
||||
dds_low_command.motor_cmd().at(i).kd() = 1.0;
|
||||
}
|
||||
|
||||
time_ += control_dt_;
|
||||
if (time_ < duration_) {
|
||||
// [Stage 1]: set robot to zero posture
|
||||
for (int i = 0; i < H1_NUM_MOTOR; ++i) {
|
||||
double ratio = std::clamp(time_ / duration_, 0.0, 1.0);
|
||||
dds_low_command.motor_cmd().at(i).q() =
|
||||
(1. - ratio) * low_state_.motor_state().at(i).q();
|
||||
}
|
||||
} else {
|
||||
// [Stage 2]: swing ankle's PR
|
||||
mode_ = PR; // Enable PR mode
|
||||
|
||||
// generate sin/cos trajectory
|
||||
double max_P = 0.25; // [rad]
|
||||
double max_R = 0.25; // [rad]
|
||||
double t = time_ - duration_;
|
||||
double L_P_des = max_P * std::cos(2.0 * M_PI * t);
|
||||
double L_R_des = max_R * std::sin(2.0 * M_PI * t);
|
||||
double R_P_des = max_P * std::cos(2.0 * M_PI * t);
|
||||
double R_R_des = -max_R * std::sin(2.0 * M_PI * t);
|
||||
|
||||
// update ankle joint position targets
|
||||
float Kp_Pitch = 80;
|
||||
float Kd_Pitch = 1;
|
||||
float Kp_Roll = 80;
|
||||
float Kd_Roll = 1;
|
||||
dds_low_command.motor_cmd().at(4).q() = L_P_des; // 4: LeftAnklePitch
|
||||
dds_low_command.motor_cmd().at(4).dq() = 0;
|
||||
dds_low_command.motor_cmd().at(4).kp() = Kp_Pitch;
|
||||
dds_low_command.motor_cmd().at(4).kd() = Kd_Pitch;
|
||||
dds_low_command.motor_cmd().at(4).tau() = 0;
|
||||
dds_low_command.motor_cmd().at(5).q() = L_R_des; // 5: LeftAnkleRoll
|
||||
dds_low_command.motor_cmd().at(5).dq() = 0;
|
||||
dds_low_command.motor_cmd().at(5).kp() = Kp_Roll;
|
||||
dds_low_command.motor_cmd().at(5).kd() = Kd_Roll;
|
||||
dds_low_command.motor_cmd().at(5).tau() = 0;
|
||||
dds_low_command.motor_cmd().at(10).q() = R_P_des; // 10: RightAnklePitch
|
||||
dds_low_command.motor_cmd().at(10).dq() = 0;
|
||||
dds_low_command.motor_cmd().at(10).kp() = Kp_Pitch;
|
||||
dds_low_command.motor_cmd().at(10).kd() = Kd_Pitch;
|
||||
dds_low_command.motor_cmd().at(10).tau() = 0;
|
||||
dds_low_command.motor_cmd().at(11).q() = R_R_des; // 11: RightAnkleRoll
|
||||
dds_low_command.motor_cmd().at(11).dq() = 0;
|
||||
dds_low_command.motor_cmd().at(11).kp() = Kp_Roll;
|
||||
dds_low_command.motor_cmd().at(11).kd() = Kd_Roll;
|
||||
dds_low_command.motor_cmd().at(11).tau() = 0;
|
||||
|
||||
float L_P_m = low_state_.motor_state().at(4).q();
|
||||
float L_R_m = low_state_.motor_state().at(5).q();
|
||||
float R_P_m = low_state_.motor_state().at(10).q();
|
||||
float R_R_m = low_state_.motor_state().at(11).q();
|
||||
// clang-format off
|
||||
printf("%f,%f,%f,%f,%f,%f,%f,%f\n",
|
||||
L_P_des, L_P_m, L_R_des, L_R_m,
|
||||
R_P_des, R_P_m, R_R_des, R_R_m);
|
||||
// clang-format on
|
||||
}
|
||||
|
||||
dds_low_command.crc() = Crc32Core((uint32_t *)&dds_low_command,
|
||||
(sizeof(dds_low_command) >> 2) - 1);
|
||||
lowcmd_publisher_->Write(dds_low_command);
|
||||
}
|
||||
};
|
||||
|
||||
int main(int argc, char const *argv[]) {
|
||||
if (argc < 2) {
|
||||
std::cout << "Usage: h1_2_ankle_track network_interface_name" << std::endl;
|
||||
exit(0);
|
||||
}
|
||||
std::string networkInterface = argv[1];
|
||||
H1Example custom(networkInterface);
|
||||
while (true) sleep(10);
|
||||
return 0;
|
||||
}
|
||||
14
unitree_sdk2/example/h1/low_level/humanoid.cpp
Normal file
14
unitree_sdk2/example/h1/low_level/humanoid.cpp
Normal file
@@ -0,0 +1,14 @@
|
||||
#include "humanoid.hpp"
|
||||
|
||||
int main(int argc, char const *argv[]) {
|
||||
if (argc < 2) {
|
||||
std::cout << "Usage: " << argv[0] << " networkInterface" << std::endl;
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
HumanoidExample example(argv[1]);
|
||||
while (1) {
|
||||
sleep(10);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
263
unitree_sdk2/example/h1/low_level/humanoid.hpp
Normal file
263
unitree_sdk2/example/h1/low_level/humanoid.hpp
Normal file
@@ -0,0 +1,263 @@
|
||||
#pragma once
|
||||
|
||||
#include <iostream>
|
||||
#include <stdint.h>
|
||||
#include <string>
|
||||
|
||||
#include "unitree/robot/channel/channel_publisher.hpp"
|
||||
#include "unitree/robot/channel/channel_subscriber.hpp"
|
||||
#include <unitree/common/thread/thread.hpp>
|
||||
#include <unitree/idl/go2/LowCmd_.hpp>
|
||||
#include <unitree/idl/go2/LowState_.hpp>
|
||||
|
||||
#include "base_state.h"
|
||||
#include "data_buffer.hpp"
|
||||
#include "motors.hpp"
|
||||
|
||||
#include <unitree/robot/b2/motion_switcher/motion_switcher_client.hpp>
|
||||
using namespace unitree::robot::b2;
|
||||
|
||||
static const std::string kTopicLowCommand = "rt/lowcmd";
|
||||
static const std::string kTopicLowState = "rt/lowstate";
|
||||
|
||||
class HumanoidExample {
|
||||
public:
|
||||
HumanoidExample(const std::string &networkInterface = "") {
|
||||
unitree::robot::ChannelFactory::Instance()->Init(0, networkInterface);
|
||||
std::cout << "Initialize channel factory." << std::endl;
|
||||
|
||||
msc.reset(new MotionSwitcherClient());
|
||||
msc->SetTimeout(5.0F);
|
||||
msc->Init();
|
||||
|
||||
/*Shut down motion control-related service*/
|
||||
while(queryMotionStatus())
|
||||
{
|
||||
std::cout << "Try to deactivate the motion control-related service." << std::endl;
|
||||
int32_t ret = msc->ReleaseMode();
|
||||
if (ret == 0) {
|
||||
std::cout << "ReleaseMode succeeded." << std::endl;
|
||||
} else {
|
||||
std::cout << "ReleaseMode failed. Error code: " << ret << std::endl;
|
||||
}
|
||||
sleep(5);
|
||||
}
|
||||
|
||||
lowcmd_publisher_.reset(
|
||||
new unitree::robot::ChannelPublisher<unitree_go::msg::dds_::LowCmd_>(
|
||||
kTopicLowCommand));
|
||||
lowcmd_publisher_->InitChannel();
|
||||
command_writer_ptr_ = unitree::common::CreateRecurrentThreadEx(
|
||||
"command_writer", UT_CPU_ID_NONE, 2000,
|
||||
&HumanoidExample::LowCommandWriter, this);
|
||||
|
||||
lowstate_subscriber_.reset(
|
||||
new unitree::robot::ChannelSubscriber<unitree_go::msg::dds_::LowState_>(
|
||||
kTopicLowState));
|
||||
lowstate_subscriber_->InitChannel(
|
||||
std::bind(&HumanoidExample::LowStateHandler, this,
|
||||
std::placeholders::_1),
|
||||
1);
|
||||
int control_period_us = control_dt_ * 1e6;
|
||||
control_thread_ptr_ = unitree::common::CreateRecurrentThreadEx(
|
||||
"control", UT_CPU_ID_NONE, control_period_us, &HumanoidExample::Control,
|
||||
this);
|
||||
|
||||
int report_period_us = report_dt_ * 1e6;
|
||||
report_rpy_ptr_ = unitree::common::CreateRecurrentThreadEx(
|
||||
"report_rpy", UT_CPU_ID_NONE, report_period_us,
|
||||
&HumanoidExample::ReportRPY, this);
|
||||
}
|
||||
|
||||
~HumanoidExample() = default;
|
||||
|
||||
void LowCommandWriter() {
|
||||
unitree_go::msg::dds_::LowCmd_ dds_low_command{};
|
||||
dds_low_command.head()[0] = 0xFE;
|
||||
dds_low_command.head()[1] = 0xEF;
|
||||
dds_low_command.level_flag() = 0xFF;
|
||||
dds_low_command.gpio() = 0;
|
||||
|
||||
const std::shared_ptr<const MotorCommand> mc_tmp_ptr =
|
||||
motor_command_buffer_.GetData();
|
||||
if (mc_tmp_ptr) {
|
||||
for (int i = 0; i < kNumMotors; ++i) {
|
||||
if (IsWeakMotor(i)) {
|
||||
dds_low_command.motor_cmd().at(i).mode() = (0x01);
|
||||
} else {
|
||||
dds_low_command.motor_cmd().at(i).mode() = (0x0A);
|
||||
}
|
||||
dds_low_command.motor_cmd().at(i).tau() = mc_tmp_ptr->tau_ff.at(i);
|
||||
dds_low_command.motor_cmd().at(i).q() = mc_tmp_ptr->q_ref.at(i);
|
||||
dds_low_command.motor_cmd().at(i).dq() = mc_tmp_ptr->dq_ref.at(i);
|
||||
dds_low_command.motor_cmd().at(i).kp() = mc_tmp_ptr->kp.at(i);
|
||||
dds_low_command.motor_cmd().at(i).kd() = mc_tmp_ptr->kd.at(i);
|
||||
}
|
||||
dds_low_command.crc() = Crc32Core((uint32_t *)&dds_low_command,
|
||||
(sizeof(dds_low_command) >> 2) - 1);
|
||||
lowcmd_publisher_->Write(dds_low_command);
|
||||
}
|
||||
}
|
||||
|
||||
void LowStateHandler(const void *message) {
|
||||
unitree_go::msg::dds_::LowState_ low_state =
|
||||
*(unitree_go::msg::dds_::LowState_ *)message;
|
||||
|
||||
RecordMotorState(low_state);
|
||||
RecordBaseState(low_state);
|
||||
}
|
||||
|
||||
void Control() {
|
||||
MotorCommand motor_command_tmp;
|
||||
const std::shared_ptr<const MotorState> ms_tmp_ptr =
|
||||
motor_state_buffer_.GetData();
|
||||
|
||||
if (ms_tmp_ptr) {
|
||||
time_ += control_dt_;
|
||||
time_ = std::clamp(time_, 0.f, init_duration_);
|
||||
float ratio = time_ / init_duration_;
|
||||
for (int i = 0; i < kNumMotors; ++i) {
|
||||
motor_command_tmp.kp.at(i) = IsWeakMotor(i) ? kp_low_ : kp_high_;
|
||||
motor_command_tmp.kd.at(i) = IsWeakMotor(i) ? kd_low_ : kd_high_;
|
||||
motor_command_tmp.dq_ref.at(i) = 0.f;
|
||||
motor_command_tmp.tau_ff.at(i) = 0.f;
|
||||
|
||||
float q_des = 0.f;
|
||||
if (i == JointIndex::kLeftHipPitch || i == JointIndex::kRightHipPitch) {
|
||||
q_des = hip_pitch_init_pos_;
|
||||
}
|
||||
if (i == JointIndex::kLeftKnee || i == JointIndex::kRightKnee) {
|
||||
q_des = knee_init_pos_;
|
||||
}
|
||||
if (i == JointIndex::kLeftAnkle || i == JointIndex::kRightAnkle) {
|
||||
q_des = ankle_init_pos_;
|
||||
}
|
||||
if (i == JointIndex::kLeftShoulderPitch ||
|
||||
i == JointIndex::kRightShoulderPitch) {
|
||||
q_des = shoulder_pitch_init_pos_;
|
||||
}
|
||||
|
||||
q_des = (q_des - ms_tmp_ptr->q.at(i)) * ratio + ms_tmp_ptr->q.at(i);
|
||||
motor_command_tmp.q_ref.at(i) = q_des;
|
||||
}
|
||||
|
||||
motor_command_buffer_.SetData(motor_command_tmp);
|
||||
}
|
||||
}
|
||||
|
||||
void ReportRPY() {
|
||||
const std::shared_ptr<const BaseState> bs_tmp_ptr =
|
||||
base_state_buffer_.GetData();
|
||||
if (bs_tmp_ptr) {
|
||||
std::cout << "rpy: [" << bs_tmp_ptr->rpy.at(0) << ", "
|
||||
<< bs_tmp_ptr->rpy.at(1) << ", " << bs_tmp_ptr->rpy.at(2) << "]"
|
||||
<< std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
void RecordMotorState(const unitree_go::msg::dds_::LowState_ &msg) {
|
||||
MotorState ms_tmp;
|
||||
for (int i = 0; i < kNumMotors; ++i) {
|
||||
ms_tmp.q.at(i) = msg.motor_state()[i].q();
|
||||
ms_tmp.dq.at(i) = msg.motor_state()[i].dq();
|
||||
}
|
||||
|
||||
motor_state_buffer_.SetData(ms_tmp);
|
||||
}
|
||||
|
||||
void RecordBaseState(const unitree_go::msg::dds_::LowState_ &msg) {
|
||||
BaseState bs_tmp;
|
||||
bs_tmp.omega = msg.imu_state().gyroscope();
|
||||
bs_tmp.rpy = msg.imu_state().rpy();
|
||||
|
||||
base_state_buffer_.SetData(bs_tmp);
|
||||
}
|
||||
|
||||
inline bool IsWeakMotor(int motor_index) {
|
||||
return motor_index == JointIndex::kLeftAnkle ||
|
||||
motor_index == JointIndex::kRightAnkle ||
|
||||
motor_index == JointIndex::kRightShoulderPitch ||
|
||||
motor_index == JointIndex::kRightShoulderRoll ||
|
||||
motor_index == JointIndex::kRightShoulderYaw ||
|
||||
motor_index == JointIndex::kRightElbow ||
|
||||
motor_index == JointIndex::kLeftShoulderPitch ||
|
||||
motor_index == JointIndex::kLeftShoulderRoll ||
|
||||
motor_index == JointIndex::kLeftShoulderYaw ||
|
||||
motor_index == JointIndex::kLeftElbow;
|
||||
}
|
||||
|
||||
std::string queryServiceName(std::string form,std::string name)
|
||||
{
|
||||
if(form == "0")
|
||||
{
|
||||
if(name == "normal" ) return "sport_mode";
|
||||
if(name == "ai" ) return "ai_sport";
|
||||
if(name == "advanced" ) return "advanced_sport";
|
||||
}
|
||||
else
|
||||
{
|
||||
if(name == "ai-w" ) return "wheeled_sport(go2W)";
|
||||
if(name == "normal-w" ) return "wheeled_sport(b2W)";
|
||||
}
|
||||
return "";
|
||||
}
|
||||
|
||||
int queryMotionStatus()
|
||||
{
|
||||
std::string robotForm,motionName;
|
||||
int motionStatus;
|
||||
int32_t ret = msc->CheckMode(robotForm,motionName);
|
||||
if (ret == 0) {
|
||||
std::cout << "CheckMode succeeded." << std::endl;
|
||||
} else {
|
||||
std::cout << "CheckMode failed. Error code: " << ret << std::endl;
|
||||
}
|
||||
if(motionName.empty())
|
||||
{
|
||||
std::cout << "The motion control-related service is deactivated." << std::endl;
|
||||
motionStatus = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
std::string serviceName = queryServiceName(robotForm,motionName);
|
||||
std::cout << "Service: "<< serviceName<< " is activate" << std::endl;
|
||||
motionStatus = 1;
|
||||
}
|
||||
return motionStatus;
|
||||
}
|
||||
|
||||
unitree::robot::ChannelPublisherPtr<unitree_go::msg::dds_::LowCmd_>
|
||||
lowcmd_publisher_;
|
||||
unitree::robot::ChannelSubscriberPtr<unitree_go::msg::dds_::LowState_>
|
||||
lowstate_subscriber_;
|
||||
|
||||
DataBuffer<MotorState> motor_state_buffer_;
|
||||
DataBuffer<MotorCommand> motor_command_buffer_;
|
||||
DataBuffer<BaseState> base_state_buffer_;
|
||||
|
||||
std::shared_ptr<MotionSwitcherClient> msc;
|
||||
|
||||
// control params
|
||||
float kp_low_ = 60.f;
|
||||
float kp_high_ = 200.f;
|
||||
float kd_low_ = 1.5f;
|
||||
float kd_high_ = 5.f;
|
||||
|
||||
float control_dt_ = 0.01f;
|
||||
|
||||
float hip_pitch_init_pos_ = -0.5f;
|
||||
float knee_init_pos_ = 1.f;
|
||||
float ankle_init_pos_ = -0.5f;
|
||||
float shoulder_pitch_init_pos_ = 0.4f;
|
||||
|
||||
float time_ = 0.f;
|
||||
float init_duration_ = 10.f;
|
||||
|
||||
float report_dt_ = 0.1f;
|
||||
|
||||
// multithreading
|
||||
unitree::common::ThreadPtr command_writer_ptr_;
|
||||
unitree::common::ThreadPtr control_thread_ptr_;
|
||||
unitree::common::ThreadPtr report_rpy_ptr_;
|
||||
};
|
||||
75
unitree_sdk2/example/h1/low_level/motors.hpp
Normal file
75
unitree_sdk2/example/h1/low_level/motors.hpp
Normal file
@@ -0,0 +1,75 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <stdint.h>
|
||||
|
||||
#include <unitree/idl/go2/LowCmd_.hpp>
|
||||
|
||||
constexpr int kNumMotors = 20;
|
||||
|
||||
struct MotorCommand {
|
||||
std::array<float, kNumMotors> q_ref = {};
|
||||
std::array<float, kNumMotors> dq_ref = {};
|
||||
std::array<float, kNumMotors> kp = {};
|
||||
std::array<float, kNumMotors> kd = {};
|
||||
std::array<float, kNumMotors> tau_ff = {};
|
||||
};
|
||||
|
||||
struct MotorState {
|
||||
std::array<float, kNumMotors> q = {};
|
||||
std::array<float, kNumMotors> dq = {};
|
||||
};
|
||||
|
||||
enum JointIndex {
|
||||
// Right leg
|
||||
kRightHipYaw = 8,
|
||||
kRightHipRoll = 0,
|
||||
kRightHipPitch = 1,
|
||||
kRightKnee = 2,
|
||||
kRightAnkle = 11,
|
||||
// Left leg
|
||||
kLeftHipYaw = 7,
|
||||
kLeftHipRoll = 3,
|
||||
kLeftHipPitch = 4,
|
||||
kLeftKnee = 5,
|
||||
kLeftAnkle = 10,
|
||||
|
||||
kWaistYaw = 6,
|
||||
|
||||
kNotUsedJoint = 9,
|
||||
|
||||
// Right arm
|
||||
kRightShoulderPitch = 12,
|
||||
kRightShoulderRoll = 13,
|
||||
kRightShoulderYaw = 14,
|
||||
kRightElbow = 15,
|
||||
// Left arm
|
||||
kLeftShoulderPitch = 16,
|
||||
kLeftShoulderRoll = 17,
|
||||
kLeftShoulderYaw = 18,
|
||||
kLeftElbow = 19,
|
||||
|
||||
};
|
||||
|
||||
uint32_t Crc32Core(uint32_t *ptr, uint32_t len) {
|
||||
uint32_t xbit = 0;
|
||||
uint32_t data = 0;
|
||||
uint32_t CRC32 = 0xFFFFFFFF;
|
||||
const uint32_t dwPolynomial = 0x04c11db7;
|
||||
for (uint32_t i = 0; i < len; i++) {
|
||||
xbit = 1 << 31;
|
||||
data = ptr[i];
|
||||
for (uint32_t bits = 0; bits < 32; bits++) {
|
||||
if (CRC32 & 0x80000000) {
|
||||
CRC32 <<= 1;
|
||||
CRC32 ^= dwPolynomial;
|
||||
} else
|
||||
CRC32 <<= 1;
|
||||
if (data & xbit)
|
||||
CRC32 ^= dwPolynomial;
|
||||
|
||||
xbit >>= 1;
|
||||
}
|
||||
}
|
||||
return CRC32;
|
||||
};
|
||||
Reference in New Issue
Block a user