Skip to content

Commit

Permalink
add gamepad in g1_ankle_swing_example
Browse files Browse the repository at this point in the history
  • Loading branch information
matheecs committed Oct 31, 2024
1 parent 69d1072 commit f1f1d85
Show file tree
Hide file tree
Showing 2 changed files with 176 additions and 8 deletions.
56 changes: 48 additions & 8 deletions example/g1/low_level/g1_ankle_swing_example.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,8 @@
#include <mutex>
#include <shared_mutex>

#include "gamepad.hpp"

// DDS
#include <unitree/robot/channel/channel_publisher.hpp>
#include <unitree/robot/channel/channel_subscriber.hpp>
Expand Down Expand Up @@ -147,9 +149,13 @@ class G1Example {
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_;
Expand All @@ -165,6 +171,7 @@ class G1Example {
: time_(0.0),
control_dt_(0.002),
duration_(3.0),
counter_(0),
mode_pr_(Mode::PR),
mode_machine_(0) {
ChannelFactory::Instance()->Init(0, networkInterface);
Expand All @@ -191,14 +198,8 @@ class G1Example {
control_thread_ptr_ = CreateRecurrentThreadEx("control", UT_CPU_ID_NONE, 2000, &G1Example::Control, this);
}

void ReportRPY() {
const std::shared_ptr<const ImuState> imu = imu_state_buffer_.GetData();
if (imu) std::cout << "rpy: [" << imu->rpy.at(0) << ", " << imu->rpy.at(1) << ", " << imu->rpy.at(2) << "]" << std::endl;
}

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;
Expand All @@ -209,7 +210,6 @@ class G1Example {
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";
}
Expand All @@ -221,11 +221,52 @@ class G1Example {
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.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() {
Expand All @@ -250,7 +291,6 @@ class G1Example {
}

void Control() {
ReportRPY();
MotorCommand motor_command_tmp;
const std::shared_ptr<const MotorState> ms = motor_state_buffer_.GetData();

Expand Down
128 changes: 128 additions & 0 deletions example/g1/low_level/gamepad.hpp
View file
Original file line number Diff line number Diff line change
@@ -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

0 comments on commit f1f1d85

Please sign in to comment.