lx/Go2_navigation
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

初始化项目，包含fast_lio_ws和unitree_sdk2

Browse Source
This commit is contained in:
龙正扬应用学生后端
2026-10-12 15:01:52 +08:00
commit 8e16ecceba
1490 changed files with 232338 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11478
unitree_sdk2/example/g1/low_level/behavior_lib/motion.seq Normal file
View File

File diff suppressed because it is too large Load Diff

371
unitree_sdk2/example/g1/low_level/g1_ankle_swing_example.cpp Normal file
View File

@@ -0,0 +1,371 @@
#include <cmath>
#include <memory>
#include <mutex>
#include <shared_mutex>
#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 G1_NUM_MOTOR = 29;
struct ImuState {
std::array<float, 3> rpy = {};
std::array<float, 3> omega = {};
};
struct MotorCommand {
std::array<float, G1_NUM_MOTOR> q_target = {};
std::array<float, G1_NUM_MOTOR> dq_target = {};
std::array<float, G1_NUM_MOTOR> kp = {};
std::array<float, G1_NUM_MOTOR> kd = {};
std::array<float, G1_NUM_MOTOR> tau_ff = {};
};
struct MotorState {
std::array<float, G1_NUM_MOTOR> q = {};
std::array<float, G1_NUM_MOTOR> dq = {};
};
// Stiffness for all G1 Joints
std::array<float, G1_NUM_MOTOR> Kp{
60, 60, 60, 100, 40, 40, // legs
60, 60, 60, 100, 40, 40, // legs
60, 40, 40, // waist
40, 40, 40, 40, 40, 40, 40, // arms
40, 40, 40, 40, 40, 40, 40 // arms
};
// Damping for all G1 Joints
std::array<float, G1_NUM_MOTOR> Kd{
1, 1, 1, 2, 1, 1, // legs
1, 1, 1, 2, 1, 1, // legs
1, 1, 1, // waist
1, 1, 1, 1, 1, 1, 1, // arms
1, 1, 1, 1, 1, 1, 1 // arms
};
enum class Mode {
PR = 0, // Series Control for Ptich/Roll Joints
AB = 1 // Parallel Control for A/B Joints
};
enum G1JointIndex {
LeftHipPitch = 0,
LeftHipRoll = 1,
LeftHipYaw = 2,
LeftKnee = 3,
LeftAnklePitch = 4,
LeftAnkleB = 4,
LeftAnkleRoll = 5,
LeftAnkleA = 5,
RightHipPitch = 6,
RightHipRoll = 7,
RightHipYaw = 8,
RightKnee = 9,
RightAnklePitch = 10,
RightAnkleB = 10,
RightAnkleRoll = 11,
RightAnkleA = 11,
WaistYaw = 12,
WaistRoll = 13, // NOTE INVALID for g1 23dof/29dof with waist locked
WaistA = 13, // NOTE INVALID for g1 23dof/29dof with waist locked
WaistPitch = 14, // NOTE INVALID for g1 23dof/29dof with waist locked
WaistB = 14, // NOTE INVALID for g1 23dof/29dof with waist locked
LeftShoulderPitch = 15,
LeftShoulderRoll = 16,
LeftShoulderYaw = 17,
LeftElbow = 18,
LeftWristRoll = 19,
LeftWristPitch = 20, // NOTE INVALID for g1 23dof
LeftWristYaw = 21, // NOTE INVALID for g1 23dof
RightShoulderPitch = 22,
RightShoulderRoll = 23,
RightShoulderYaw = 24,
RightElbow = 25,
RightWristRoll = 26,
RightWristPitch = 27, // NOTE INVALID for g1 23dof
RightWristYaw = 28 // NOTE INVALID for g1 23dof
};
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 G1Example {
private:
double time_;
double control_dt_; // [2ms]
double duration_; // [3 s]
int counter_;
Mode mode_pr_;
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_;
ChannelPublisherPtr<LowCmd_> lowcmd_publisher_;
ChannelSubscriberPtr<LowState_> lowstate_subscriber_;
ChannelSubscriberPtr<IMUState_> imutorso_subscriber_;
ThreadPtr command_writer_ptr_, control_thread_ptr_;
std::shared_ptr<unitree::robot::b2::MotionSwitcherClient> msc_;
public:
G1Example(std::string networkInterface)
: time_(0.0),
control_dt_(0.002),
duration_(3.0),
counter_(0),
mode_pr_(Mode::PR),
mode_machine_(0) {
ChannelFactory::Instance()->Init(0, networkInterface);
// try to shutdown motion control-related service
msc_ = std::make_shared<unitree::robot::b2::MotionSwitcherClient>();
msc_->SetTimeout(5.0f);
msc_->Init();
std::string form, name;
while (msc_->CheckMode(form, name), !name.empty()) {
if (msc_->ReleaseMode())
std::cout << "Failed to switch to Release Mode\n";
sleep(5);
}
// create publisher
lowcmd_publisher_.reset(new ChannelPublisher<LowCmd_>(HG_CMD_TOPIC));
lowcmd_publisher_->InitChannel();
// create subscriber
lowstate_subscriber_.reset(new ChannelSubscriber<LowState_>(HG_STATE_TOPIC));
lowstate_subscriber_->InitChannel(std::bind(&G1Example::LowStateHandler, this, std::placeholders::_1), 1);
imutorso_subscriber_.reset(new ChannelSubscriber<IMUState_>(HG_IMU_TORSO));
imutorso_subscriber_->InitChannel(std::bind(&G1Example::imuTorsoHandler, this, std::placeholders::_1), 1);
// create threads
command_writer_ptr_ = CreateRecurrentThreadEx("command_writer", UT_CPU_ID_NONE, 2000, &G1Example::LowCommandWriter, this);
control_thread_ptr_ = CreateRecurrentThreadEx("control", UT_CPU_ID_NONE, 2000, &G1Example::Control, this);
}
void imuTorsoHandler(const void *message) {
IMUState_ imu_torso = *(const IMUState_ *)message;
auto &rpy = imu_torso.rpy();
if (counter_ % 500 == 0)
printf("IMU.torso.rpy: %.2f %.2f %.2f\n", rpy[0], rpy[1], rpy[2]);
}
void LowStateHandler(const void *message) {
LowState_ low_state = *(const LowState_ *)message;
if (low_state.crc() != Crc32Core((uint32_t *)&low_state, (sizeof(LowState_) >> 2) - 1)) {
std::cout << "[ERROR] CRC Error" << std::endl;
return;
}
// get motor state
MotorState ms_tmp;
for (int i = 0; i < G1_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);
// 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 gamepad
memcpy(rx_.buff, &low_state.wireless_remote()[0], 40);
gamepad_.update(rx_.RF_RX);
// 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();
}
// report robot status every second
if (++counter_ % 500 == 0) {
counter_ = 0;
// IMU
auto &rpy = low_state.imu_state().rpy();
printf("IMU.pelvis.rpy: %.2f %.2f %.2f\n", rpy[0], rpy[1], rpy[2]);
// RC
printf("gamepad_.A.pressed: %d\n", static_cast<int>(gamepad_.A.pressed));
printf("gamepad_.B.pressed: %d\n", static_cast<int>(gamepad_.B.pressed));
printf("gamepad_.X.pressed: %d\n", static_cast<int>(gamepad_.X.pressed));
printf("gamepad_.Y.pressed: %d\n", static_cast<int>(gamepad_.Y.pressed));
// Motor
auto &ms = low_state.motor_state();
printf("All %d Motors:", G1_NUM_MOTOR);
printf("\nmode: ");
for (int i = 0; i < G1_NUM_MOTOR; ++i) printf("%u,", ms[i].mode());
printf("\npos: ");
for (int i = 0; i < G1_NUM_MOTOR; ++i) printf("%.2f,", ms[i].q());
printf("\nvel: ");
for (int i = 0; i < G1_NUM_MOTOR; ++i) printf("%.2f,", ms[i].dq());
printf("\ntau_est: ");
for (int i = 0; i < G1_NUM_MOTOR; ++i) printf("%.2f,", ms[i].tau_est());
printf("\ntemperature: ");
for (int i = 0; i < G1_NUM_MOTOR; ++i) printf("%d,%d;", ms[i].temperature()[0], ms[i].temperature()[1]);
printf("\nvol: ");
for (int i = 0; i < G1_NUM_MOTOR; ++i) printf("%.2f,", ms[i].vol());
printf("\nsensor: ");
for (int i = 0; i < G1_NUM_MOTOR; ++i) printf("%u,%u;", ms[i].sensor()[0], ms[i].sensor()[1]);
printf("\nmotorstate: ");
for (int i = 0; i < G1_NUM_MOTOR; ++i) printf("%u,", ms[i].motorstate());
printf("\nreserve: ");
for (int i = 0; i < G1_NUM_MOTOR; ++i) printf("%u,%u,%u,%u;", ms[i].reserve()[0], ms[i].reserve()[1], ms[i].reserve()[2], ms[i].reserve()[3]);
printf("\n");
}
}
void LowCommandWriter() {
LowCmd_ dds_low_command;
dds_low_command.mode_pr() = static_cast<uint8_t>(mode_pr_);
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 < G1_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() {
MotorCommand motor_command_tmp;
const std::shared_ptr<const MotorState> ms = motor_state_buffer_.GetData();
for (int i = 0; i < G1_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) = Kp[i];
motor_command_tmp.kd.at(i) = Kd[i];
}
if (ms) {
time_ += control_dt_;
if (time_ < duration_) {
// [Stage 1]: set robot to zero posture
for (int i = 0; i < G1_NUM_MOTOR; ++i) {
double ratio = std::clamp(time_ / duration_, 0.0, 1.0);
motor_command_tmp.q_target.at(i) = (1.0 - ratio) * ms->q.at(i);
}
} else if (time_ < duration_ * 2) {
// [Stage 2]: swing ankle using PR mode
mode_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_;
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);
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 using AB mode
mode_pr_ = 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);
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: g1_ankle_swing_example network_interface" << std::endl;
exit(0);
}
std::string networkInterface = argv[1];
G1Example custom(networkInterface);
while (true) sleep(10);
return 0;
}

407
unitree_sdk2/example/g1/low_level/g1_dual_arm_example.cpp Normal file
View File

@@ -0,0 +1,407 @@
#include <yaml-cpp/yaml.h>
#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>
#include <unitree/robot/b2/motion_switcher/motion_switcher_client.hpp>
using namespace unitree::robot::b2;
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 G1_NUM_MOTOR = 29;
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, G1_NUM_MOTOR> q_target = {};
std::array<float, G1_NUM_MOTOR> dq_target = {};
std::array<float, G1_NUM_MOTOR> kp = {};
std::array<float, G1_NUM_MOTOR> kd = {};
std::array<float, G1_NUM_MOTOR> tau_ff = {};
};
struct MotorState {
std::array<float, G1_NUM_MOTOR> q = {};
std::array<float, G1_NUM_MOTOR> dq = {};
};
enum MotorType { GearboxS = 0, GearboxM = 1, GearboxL = 2 };
std::array<MotorType, G1_NUM_MOTOR> G1MotorType{
// clang-format off
// legs
GearboxM, GearboxM, GearboxM, GearboxL, GearboxS, GearboxS,
GearboxM, GearboxM, GearboxM, GearboxL, GearboxS, GearboxS,
// waist
GearboxM, GearboxS, GearboxS,
// 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 G1JointValidIndex {
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
};
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 40;
case GearboxM:
return 40;
case GearboxL:
return 100;
default:
return 0;
}
}
float GetMotorKd(MotorType type) {
switch (type) {
case GearboxS:
return 1;
case GearboxM:
return 1;
case GearboxL:
return 1;
default:
return 0;
}
}
class G1Example {
private:
double time_;
double control_dt_; // [2ms]
double duration_; // [3 s]
PRorAB mode_;
uint8_t mode_machine_;
std::vector<std::vector<double>> frames_data_;
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_;
std::shared_ptr<MotionSwitcherClient> msc;
public:
G1Example(std::string networkInterface)
: time_(0.0),
control_dt_(0.002),
duration_(3.0),
mode_(PR),
mode_machine_(0) {
ChannelFactory::Instance()->Init(0, networkInterface);
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);
}
loadBehaviorLibrary("motion");
// 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(&G1Example::LowStateHandler, this, std::placeholders::_1), 1);
// create threads
command_writer_ptr_ =
CreateRecurrentThreadEx("command_writer", UT_CPU_ID_NONE, 2000,
&G1Example::LowCommandWriter, this);
control_thread_ptr_ = CreateRecurrentThreadEx(
"control", UT_CPU_ID_NONE, 2000, &G1Example::Control, this);
}
void loadBehaviorLibrary(std::string behavior_name) {
std::string resource_dir = BLIB_DIR;
YAML::Node motion = YAML::LoadFile(resource_dir + behavior_name + ".seq");
std::string content = motion["components"][1]["content"].as<std::string>();
int num_parts = motion["components"][1]["num_parts"].as<int>();
std::cout << "BehaviorName: " << behavior_name + ".seq\n";
std::cout << content << " with " << num_parts << "\n";
auto frames = motion["components"][1]["frames"];
for (const auto &frame : frames) {
std::vector<double> frame_data;
for (const auto &element : frame) {
frame_data.push_back(element.as<double>());
}
frames_data_.push_back(frame_data);
}
std::cout << frames_data_.size() << " knots with " << frames_data_[0].size()
<< " DOF\n";
}
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 < G1_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();
}
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 < G1_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() {
MotorCommand motor_command_tmp;
const std::shared_ptr<const MotorState> ms = motor_state_buffer_.GetData();
if (ms) {
time_ += control_dt_;
if (time_ < duration_) {
// [Stage 1]: set robot to zero posture
for (int i = 0; i < G1_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(G1MotorType[i]);
motor_command_tmp.kd.at(i) = GetMotorKd(G1MotorType[i]);
}
} else {
// [Stage 2]: tracking the offline trajectory
size_t frame_index = (size_t)((time_ - duration_) / control_dt_);
if (frame_index >= frames_data_.size()) {
frame_index = frames_data_.size() - 1;
time_ = 0.0; // RESET
}
if (frame_index % 100 == 0)
std::cout << "Frame Index: " << frame_index << std::endl;
for (int i = 0; i < G1_NUM_MOTOR; ++i) {
size_t index_in_frame = i - LeftShoulderPitch;
motor_command_tmp.q_target.at(i) =
(i >= LeftShoulderPitch)
? frames_data_[frame_index][index_in_frame]
: 0.0;
motor_command_tmp.dq_target.at(i) = 0.0;
motor_command_tmp.tau_ff.at(i) = 0.0;
motor_command_tmp.kp.at(i) = GetMotorKp(G1MotorType[i]);
motor_command_tmp.kd.at(i) = GetMotorKd(G1MotorType[i]);
}
}
motor_command_buffer_.SetData(motor_command_tmp);
}
}
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;
}
};
int main(int argc, char const *argv[]) {
if (argc < 2) {
std::cout << "Usage: g1_dual_arm_example network_interface_name"
<< std::endl;
exit(0);
}
std::string networkInterface = argv[1];
G1Example custom(networkInterface);
while (true) sleep(10);
return 0;
}

128
unitree_sdk2/example/g1/low_level/gamepad.hpp Normal file
View File

@@ -0,0 +1,128 @@
#pragma once
#include <cmath>
namespace unitree::common
{
// bytecode mapping for raw joystick data
// 16b
typedef union
{
struct
{
uint8_t R1 : 1;
uint8_t L1 : 1;
uint8_t start : 1;
uint8_t select : 1;
uint8_t R2 : 1;
uint8_t L2 : 1;
uint8_t F1 : 1;
uint8_t F2 : 1;
uint8_t A : 1;
uint8_t B : 1;
uint8_t X : 1;
uint8_t Y : 1;
uint8_t up : 1;
uint8_t right : 1;
uint8_t down : 1;
uint8_t left : 1;
} components;
uint16_t value;
} xKeySwitchUnion;
// 40 Byte (now used 24B)
typedef struct
{
uint8_t head[2];
xKeySwitchUnion btn;
float lx;
float rx;
float ry;
float L2;
float ly;
uint8_t idle[16];
} xRockerBtnDataStruct;
typedef union
{
xRockerBtnDataStruct RF_RX;
uint8_t buff[40];
} REMOTE_DATA_RX;
class Button
{
public:
Button() {}
void update(bool state)
{
on_press = state ? state != pressed : false;
on_release = state ? false : state != pressed;
pressed = state;
}
bool pressed = false;
bool on_press = false;
bool on_release = false;
};
class Gamepad
{
public:
Gamepad() {}
void update(xRockerBtnDataStruct &key_data)
{
lx = lx * (1 - smooth) + (std::fabs(key_data.lx) < dead_zone ? 0.0 : key_data.lx) * smooth;
rx = rx * (1 - smooth) + (std::fabs(key_data.rx) < dead_zone ? 0.0 : key_data.rx) * smooth;
ry = ry * (1 - smooth) + (std::fabs(key_data.ry) < dead_zone ? 0.0 : key_data.ry) * smooth;
l2 = l2 * (1 - smooth) + (std::fabs(key_data.L2) < dead_zone ? 0.0 : key_data.L2) * smooth;
ly = ly * (1 - smooth) + (std::fabs(key_data.ly) < dead_zone ? 0.0 : key_data.ly) * smooth;
R1.update(key_data.btn.components.R1);
L1.update(key_data.btn.components.L1);
start.update(key_data.btn.components.start);
select.update(key_data.btn.components.select);
R2.update(key_data.btn.components.R2);
L2.update(key_data.btn.components.L2);
F1.update(key_data.btn.components.F1);
F2.update(key_data.btn.components.F2);
A.update(key_data.btn.components.A);
B.update(key_data.btn.components.B);
X.update(key_data.btn.components.X);
Y.update(key_data.btn.components.Y);
up.update(key_data.btn.components.up);
right.update(key_data.btn.components.right);
down.update(key_data.btn.components.down);
left.update(key_data.btn.components.left);
}
float lx = 0.;
float rx = 0.;
float ry = 0.;
float l2 = 0.;
float ly = 0.;
float smooth = 0.03;
float dead_zone = 0.01;
Button R1;
Button L1;
Button start;
Button select;
Button R2;
Button L2;
Button F1;
Button F2;
Button A;
Button B;
Button X;
Button Y;
Button up;
Button right;
Button down;
Button left;
};
} // namespace unitree::common

46
unitree_sdk2/example/g1/low_level/terminations.cpp Normal file
View File

@@ -0,0 +1,46 @@
#include <unitree/robot/g1/common/terminations.hpp>
#include <boost/program_options.hpp>
#include <thread>
namespace po = boost::program_options;
using namespace unitree::robot;
using namespace unitree_hg::msg::dds_;
int main(int argc, char** argv)
{
// Parse command line arguments
po::options_description desc("Unitree G1 termination functions testing.");
desc.add_options()
("network,n", po::value<std::string>()->default_value(""), "dds network interface")
;
po::variables_map vm;
po::store(po::parse_command_line(argc, argv, desc), vm);
po::notify(vm);
std::cout << desc << std::endl;
// DDS Init
ChannelFactory::Instance()->Init(0, vm["network"].as<std::string>());
auto lowstate_subscriber = std::make_shared<ChannelSubscriber<LowState_>>("rt/lowstate");
LowState_ lowstate;
lowstate_subscriber->InitChannel([&lowstate](const void* message) {
lowstate = *(const LowState_*)message;
});
std::cout << "Checking terminations..." << std::endl;
while (true)
{
if (g1::bad_orientation(lowstate, 1.0f)) { // Tip the robot over to test bad orientation
std::cout << "Bad orientation detected!" << std::endl;
}
if (g1::lost_connection(lowstate_subscriber, 1000)) { // Unplug the network cable to test lost connection
std::cout << "Lost connection!" << std::endl;
}
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
return 0;
}