293 lines
8.6 KiB
C++
293 lines
8.6 KiB
C++
|
#include <cmath>
|
|||
|
|
#include <memory>
|
|||
|
|
#include <mutex>
|
|||
|
|
#include <shared_mutex>
|
|||
|
|
#include <thread>
|
|||
|
|
#include <chrono>
|
|||
|
|
|
|||
|
|
#include "gamepad.hpp"
|
|||
|
|
|
|||
|
|
// DDS
|
|||
|
|
#include <unitree/robot/channel/channel_publisher.hpp>
|
|||
|
|
#include <unitree/robot/channel/channel_subscriber.hpp>
|
|||
|
|
|
|||
|
|
// IDL
|
|||
|
|
#include <unitree/idl/hg/IMUState_.hpp>
|
|||
|
|
#include <unitree/idl/hg/LowCmd_.hpp>
|
|||
|
|
#include <unitree/idl/hg/LowState_.hpp>
|
|||
|
|
#include <unitree/robot/b2/motion_switcher/motion_switcher_client.hpp>
|
|||
|
|
|
|||
|
|
static const std::string HG_CMD_TOPIC = "rt/lowcmd";
|
|||
|
|
static const std::string HG_IMU_TORSO = "rt/secondary_imu";
|
|||
|
|
static const std::string HG_STATE_TOPIC = "rt/lowstate";
|
|||
|
|
|
|||
|
|
using namespace unitree::common;
|
|||
|
|
using namespace unitree::robot;
|
|||
|
|
using namespace unitree_hg::msg::dds_;
|
|||
|
|
|
|||
|
|
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;
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
const int G1D_NUM_MOTOR = 29;
|
|||
|
|
|
|||
|
|
// IMU状态结构体
|
|||
|
|
struct ImuState {
|
|||
|
|
std::array<float, 3> rpy = {};
|
|||
|
|
std::array<float, 3> omega = {};
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
// 电机控制指令结构体
|
|||
|
|
struct MotorCommand {
|
|||
|
|
std::array<float, G1D_NUM_MOTOR> q_target = {};
|
|||
|
|
std::array<float, G1D_NUM_MOTOR> dq_target = {};
|
|||
|
|
std::array<float, G1D_NUM_MOTOR> kp = {};
|
|||
|
|
std::array<float, G1D_NUM_MOTOR> kd = {};
|
|||
|
|
std::array<float, G1D_NUM_MOTOR> tau_ff = {};
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
// 电机状态结构体
|
|||
|
|
struct MotorState {
|
|||
|
|
std::array<float, G1D_NUM_MOTOR> q = {};
|
|||
|
|
std::array<float, G1D_NUM_MOTOR> dq = {};
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
// Stiffness for all G1D Joints
|
|||
|
|
std::array<float, G1D_NUM_MOTOR> Kp{
|
|||
|
|
0, 0, 0, 0, 0, 0, // empty
|
|||
|
|
0, 0, 0, 0, 0, 0, // empty
|
|||
|
|
60, 0, 40, // waist
|
|||
|
|
40, 40, 40, 40, 40, 40, 40, // arms
|
|||
|
|
40, 40, 40, 40, 40, 40, 40 // arms
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
// Damping for all G1D Joints
|
|||
|
|
std::array<float, G1D_NUM_MOTOR> Kd{
|
|||
|
|
0, 0, 0, 0, 0, 0, // empty
|
|||
|
|
0, 0, 0, 0, 0, 0, // empty
|
|||
|
|
1, 0, 1, // waist
|
|||
|
|
1, 1, 1, 1, 1, 1, 1, // arms
|
|||
|
|
1, 1, 1, 1, 1, 1, 1 // arms
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
uint8_t mode_machine_;
|
|||
|
|
|
|||
|
|
Gamepad gamepad_;
|
|||
|
|
REMOTE_DATA_RX rx_;
|
|||
|
|
|
|||
|
|
DataBuffer<MotorState> motor_state_buffer_;
|
|||
|
|
DataBuffer<MotorCommand> motor_command_buffer_;
|
|||
|
|
DataBuffer<ImuState> imu_state_buffer_;
|
|||
|
|
|
|||
|
|
ThreadPtr command_writer_ptr_, control_thread_ptr_;
|
|||
|
|
|
|||
|
|
LowState_ g1d_low_state;
|
|||
|
|
IMUState_ Torso_IMU;
|
|||
|
|
|
|||
|
|
bool A_PRESSED = false;
|
|||
|
|
bool IMU_Get = false;
|
|||
|
|
|
|||
|
|
enum G1DJointIndex {
|
|||
|
|
LeftHipPitch = 0, // NOTE INVALID for g1d
|
|||
|
|
LeftHipRoll = 1, // NOTE INVALID for g1d
|
|||
|
|
LeftHipYaw = 2, // NOTE INVALID for g1d
|
|||
|
|
LeftKnee = 3, // NOTE INVALID for g1d
|
|||
|
|
LeftAnklePitch = 4, // NOTE INVALID for g1d
|
|||
|
|
LeftAnkleB = 4, // NOTE INVALID for g1d
|
|||
|
|
LeftAnkleRoll = 5, // NOTE INVALID for g1d
|
|||
|
|
LeftAnkleA = 5, // NOTE INVALID for g1d
|
|||
|
|
RightHipPitch = 6, // NOTE INVALID for g1d
|
|||
|
|
RightHipRoll = 7, // NOTE INVALID for g1d
|
|||
|
|
RightHipYaw = 8, // NOTE INVALID for g1d
|
|||
|
|
RightKnee = 9, // NOTE INVALID for g1d
|
|||
|
|
RightAnklePitch = 10, // NOTE INVALID for g1d
|
|||
|
|
RightAnkleB = 10, // NOTE INVALID for g1d
|
|||
|
|
RightAnkleRoll = 11, // NOTE INVALID for g1d
|
|||
|
|
RightAnkleA = 11, // NOTE INVALID for g1d
|
|||
|
|
WaistYaw = 12,
|
|||
|
|
WaistRoll = 13, // NOTE INVALID for g1d
|
|||
|
|
WaistA = 13, // NOTE INVALID for g1d
|
|||
|
|
WaistPitch = 14,
|
|||
|
|
WaistB = 14, // NOTE INVALID for g1d
|
|||
|
|
LeftShoulderPitch = 15,
|
|||
|
|
LeftShoulderRoll = 16,
|
|||
|
|
LeftShoulderYaw = 17,
|
|||
|
|
LeftElbow = 18,
|
|||
|
|
LeftWristRoll = 19,
|
|||
|
|
LeftWristPitch = 20,
|
|||
|
|
LeftWristYaw = 21,
|
|||
|
|
RightShoulderPitch = 22,
|
|||
|
|
RightShoulderRoll = 23,
|
|||
|
|
RightShoulderYaw = 24,
|
|||
|
|
RightElbow = 25,
|
|||
|
|
RightWristRoll = 26,
|
|||
|
|
RightWristPitch = 27,
|
|||
|
|
RightWristYaw = 28
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
// CRC校验值计算
|
|||
|
|
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;
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
// 躯干IMU消息回调函数
|
|||
|
|
void imuTorsoHandler(const void *message) {
|
|||
|
|
IMUState_ imu_torso = *(const IMUState_ *)message;
|
|||
|
|
Torso_IMU = imu_torso;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 底层状态消息回调函数
|
|||
|
|
void LowStateHandler(const void *message) {
|
|||
|
|
LowState_ low_state = *(const LowState_ *)message;
|
|||
|
|
g1d_low_state = low_state;
|
|||
|
|
if (low_state.crc() != Crc32Core((uint32_t *)&low_state, (sizeof(LowState_) >> 2) - 1)) {
|
|||
|
|
std::cout << "[ERROR] CRC Error" << std::endl;
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 获取关节电机数据
|
|||
|
|
MotorState ms_tmp;
|
|||
|
|
for (int i = 0; i < G1D_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() && i <= RightAnkleRoll)
|
|||
|
|
std::cout << "[ERROR] motor " << i << " with code " << low_state.motor_state()[i].motorstate() << "\n";
|
|||
|
|
}
|
|||
|
|
motor_state_buffer_.SetData(ms_tmp);
|
|||
|
|
|
|||
|
|
// 获取IMU数据
|
|||
|
|
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);
|
|||
|
|
IMU_Get = true;
|
|||
|
|
|
|||
|
|
// 更新遥控器状态
|
|||
|
|
memcpy(rx_.buff, &low_state.wireless_remote()[0], 40);
|
|||
|
|
gamepad_.update(rx_.RF_RX);
|
|||
|
|
if(gamepad_.A.on_press) A_PRESSED = true;
|
|||
|
|
|
|||
|
|
// 更新机型编号
|
|||
|
|
mode_machine_ = low_state.mode_machine();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
int main(int argc, char const *argv[]) {
|
|||
|
|
if (argc < 2) {
|
|||
|
|
std::cout << "Usage: g1d_arm_example network_interface" << std::endl;
|
|||
|
|
exit(0);
|
|||
|
|
}
|
|||
|
|
std::string networkInterface = argv[1];
|
|||
|
|
ChannelFactory::Instance()->Init(0, networkInterface);
|
|||
|
|
|
|||
|
|
// Publisher
|
|||
|
|
ChannelPublisherPtr<LowCmd_> lowcmd_publisher(new ChannelPublisher<LowCmd_>(HG_CMD_TOPIC));
|
|||
|
|
lowcmd_publisher->InitChannel();
|
|||
|
|
|
|||
|
|
// Subscriber
|
|||
|
|
ChannelSubscriberPtr<LowState_> lowstate_subscriber(new ChannelSubscriber<LowState_>(HG_STATE_TOPIC));
|
|||
|
|
lowstate_subscriber->InitChannel(LowStateHandler, 1);
|
|||
|
|
ChannelSubscriberPtr<IMUState_> imu_subscriber(new ChannelSubscriber<IMUState_>(HG_IMU_TORSO));
|
|||
|
|
imu_subscriber->InitChannel(imuTorsoHandler, 1);
|
|||
|
|
|
|||
|
|
// 等待接收到第一帧IMU数据，打印躯干IMU状态信息
|
|||
|
|
while(!IMU_Get){
|
|||
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
|||
|
|
}
|
|||
|
|
std::cout << "Torso IMU State: " << std::endl
|
|||
|
|
<< "RPY = "
|
|||
|
|
<< Torso_IMU.rpy()[0] << ", " << Torso_IMU.rpy()[1] << ", " << Torso_IMU.rpy()[2]
|
|||
|
|
<< std::endl
|
|||
|
|
<< "Gyro = "
|
|||
|
|
<< Torso_IMU.gyroscope()[0] << ", " << Torso_IMU.gyroscope()[1] << ", " << Torso_IMU.gyroscope()[2]
|
|||
|
|
<< std::endl;
|
|||
|
|
|
|||
|
|
// 等待遥控器A键按下
|
|||
|
|
std::cout << "Press button A on the remote control to continue!" << std::endl;
|
|||
|
|
while(true){
|
|||
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
|||
|
|
if(A_PRESSED){
|
|||
|
|
A_PRESSED = false;
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
std::cout << "A Pressed!" << std::endl;
|
|||
|
|
|
|||
|
|
// 手臂摆动
|
|||
|
|
LowCmd_ dds_low_command;
|
|||
|
|
dds_low_command.mode_pr() = static_cast<uint8_t>(0); //构造底层控制指令
|
|||
|
|
dds_low_command.mode_machine() = mode_machine_;
|
|||
|
|
for (size_t i = 0; i < G1D_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;
|
|||
|
|
dds_low_command.motor_cmd().at(i).q() = motor_state_buffer_.GetData() -> q.at(i);
|
|||
|
|
dds_low_command.motor_cmd().at(i).dq() = 0;
|
|||
|
|
dds_low_command.motor_cmd().at(i).kp() = Kp.at(i);
|
|||
|
|
dds_low_command.motor_cmd().at(i).kd() = Kd.at(i);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
double control_dt = 0.002;
|
|||
|
|
double max_P = M_PI * 30.0 / 180.0;
|
|||
|
|
double t = 0.0;
|
|||
|
|
|
|||
|
|
std::cout << "Press button A on the remote control to exit the program!" << std::endl;
|
|||
|
|
while(true){
|
|||
|
|
double L_Shoulder_des = max_P * std::sin(2.0 * M_PI * t);
|
|||
|
|
double R_Shoulder_des = -max_P * std::sin(2.0 * M_PI * t);
|
|||
|
|
|
|||
|
|
dds_low_command.motor_cmd().at(LeftShoulderPitch).q() = L_Shoulder_des;
|
|||
|
|
dds_low_command.motor_cmd().at(RightShoulderPitch).q() = R_Shoulder_des;
|
|||
|
|
|
|||
|
|
dds_low_command.crc() = Crc32Core((uint32_t *)&dds_low_command, (sizeof(dds_low_command) >> 2) - 1);
|
|||
|
|
lowcmd_publisher -> Write(dds_low_command); //发布底层控制指令
|
|||
|
|
|
|||
|
|
t += control_dt;
|
|||
|
|
|
|||
|
|
// 按下遥控器A键或键盘Ctrl+C退出程序
|
|||
|
|
if(A_PRESSED){
|
|||
|
|
A_PRESSED = false;
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(5));
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return 0;
|
|||
|
|
}
|