From 194435355ce3d1720242aa7935f441c6d2aefdcb Mon Sep 17 00:00:00 2001 From: zma69650 <2225988766@qq.com> Date: Mon, 14 Oct 2024 10:43:48 +0800 Subject: [PATCH] fix bug --- src/rl_sar/CMakeLists.txt | 3 ++ src/rl_sar/include/rl_real_go2.hpp | 36 ++++++++++++++- src/rl_sar/models/go2_isaacgym/config.yaml | 16 +++---- src/rl_sar/src/rl_real_go2.cpp | 54 +++++++++++----------- 4 files changed, 73 insertions(+), 36 deletions(-) diff --git a/src/rl_sar/CMakeLists.txt b/src/rl_sar/CMakeLists.txt index 77ecf41..b2cc6c7 100644 --- a/src/rl_sar/CMakeLists.txt +++ b/src/rl_sar/CMakeLists.txt @@ -37,6 +37,9 @@ catkin_package( rospy ) +link_directories(/usr/local/lib) +include_directories(${YAML_CPP_INCLUDE_DIR}) + # Unitree A1 include_directories(library/unitree_legged_sdk_3.2/include) link_directories(library/unitree_legged_sdk_3.2/lib) diff --git a/src/rl_sar/include/rl_real_go2.hpp b/src/rl_sar/include/rl_real_go2.hpp index 3d5f21b..18e7195 100644 --- a/src/rl_sar/include/rl_real_go2.hpp +++ b/src/rl_sar/include/rl_real_go2.hpp @@ -8,6 +8,8 @@ #include #include #include +#include +#include #include #include #include @@ -22,9 +24,37 @@ using namespace unitree::robot; using namespace unitree::robot::go2; #define TOPIC_LOWCMD "rt/lowcmd" #define TOPIC_LOWSTATE "rt/lowstate" +#define TOPIC_JOYSTICK "rt/wirelesscontroller" constexpr double PosStopF = (2.146E+9f); constexpr double VelStopF = (16000.0f); +// 遥控器键值联合体 +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; + + + class RL_Real : public RL { public: @@ -59,16 +89,18 @@ private: void InitRobotStateClient(); int QueryServiceStatus(const std::string &serviceName); void ActivateService(const std::string &serviceName, int activate); - void LowCmdwriteHandler(); uint32_t Crc32Core(uint32_t *ptr, uint32_t len); void InitLowCmd(); void LowStateMessageHandler(const void *messages); + void JoystickHandler(const void *message); RobotStateClient rsc; unitree_go::msg::dds_::LowCmd_ unitree_low_command{}; // default init unitree_go::msg::dds_::LowState_ unitree_low_state{}; // default init + unitree_go::msg::dds_::WirelessController_ joystick{}; ChannelPublisherPtr lowcmd_publisher; // publisher ChannelSubscriberPtr lowstate_subscriber; //subscriber - // xRockerBtnDataStruct unitree_joy; // TODO-devel-go2 go2键盘接口 + ChannelSubscriberPtr joystick_subscriber; + xKeySwitchUnion unitree_joy; // others int motiontime = 0; diff --git a/src/rl_sar/models/go2_isaacgym/config.yaml b/src/rl_sar/models/go2_isaacgym/config.yaml index 02eccf6..2f18441 100644 --- a/src/rl_sar/models/go2_isaacgym/config.yaml +++ b/src/rl_sar/models/go2_isaacgym/config.yaml @@ -17,14 +17,14 @@ go2_isaacgym: 100, 100, 100, 100, 100, 100, 100, 100, 100] - rl_kp: [30, 30, 30, - 30, 30, 30, - 30, 30, 30, - 30, 30, 30] - rl_kd: [0.75, 0.75, 0.75, - 0.75, 0.75, 0.75, - 0.75, 0.75, 0.75, - 0.75, 0.75, 0.75] + rl_kp: [40, 40, 40, + 40, 40, 40, + 40, 40, 40, + 40, 40, 40] + rl_kd: [1, 1, 1, + 1, 1, 1, + 1, 1, 1, + 1, 1, 1] fixed_kp: [60, 60, 60, 60, 60, 60, 60, 60, 60, diff --git a/src/rl_sar/src/rl_real_go2.cpp b/src/rl_sar/src/rl_real_go2.cpp index 03beb2a..9825adb 100644 --- a/src/rl_sar/src/rl_real_go2.cpp +++ b/src/rl_sar/src/rl_real_go2.cpp @@ -5,6 +5,8 @@ RL_Real::RL_Real(const std::string &network_interface) { + std::cout<<"000000"<robot_name = "go2_isaacgym"; this->ReadYaml(this->robot_name); @@ -16,7 +18,7 @@ RL_Real::RL_Real(const std::string &network_interface) observation = "ang_vel_body"; } } - + std::cout<<"000000"<Init(0, network_interface); this->InitRobotStateClient(); @@ -33,8 +35,10 @@ RL_Real::RL_Real(const std::string &network_interface) // create subscriber lowstate_subscriber.reset(new ChannelSubscriber(TOPIC_LOWSTATE)); lowstate_subscriber->InitChannel(std::bind(&RL_Real::LowStateMessageHandler, this, std::placeholders::_1), 1); - // loop publishing thread TODO-devel-go2 why? - // lowCmdWriteThreadPtr = CreateRecurrentThreadEx("writebasiccmd", UT_CPU_ID_NONE, 2000, &RL_Real::LowCmdwriteHandler, this); + + joystick_subscriber.reset(new ChannelSubscriber(TOPIC_JOYSTICK)); + joystick_subscriber->InitChannel(std::bind(&RL_Real::JoystickHandler, this, std::placeholders::_1), 1); + // init rl torch::autograd::GradMode::set_enabled(false); @@ -86,21 +90,18 @@ RL_Real::~RL_Real() void RL_Real::GetState(RobotState *state) { - // TODO-devel-go2 go2键盘接口 - // memcpy(&this->unitree_joy, &this->unitree_low_state.wireless_remote()[0], 40); - - // if ((int)this->unitree_joy.btn.components.R2 == 1) - // { - // this->control.control_state = STATE_POS_GETUP; - // } - // else if ((int)this->unitree_joy.btn.components.R1 == 1) - // { - // this->control.control_state = STATE_RL_INIT; - // } - // else if ((int)this->unitree_joy.btn.components.L2 == 1) - // { - // this->control.control_state = STATE_POS_GETDOWN; - // } + if ((int)this->unitree_joy.components.R2 == 1) + { + this->control.control_state = STATE_POS_GETUP; + } + else if ((int)this->unitree_joy.components.R1 == 1) + { + this->control.control_state = STATE_RL_INIT; + } + else if ((int)this->unitree_joy.components.L2 == 1) + { + this->control.control_state = STATE_POS_GETDOWN; + } if (this->params.framework == "isaacgym") { @@ -141,7 +142,6 @@ void RL_Real::SetCommand(const RobotCommand *command) this->unitree_low_command.motor_cmd()[i].tau() = command->motor_command.tau[command_mapping[i]]; } - // 暂时不发 TODO-devel-go2 Why? this->unitree_low_command.crc() = Crc32Core((uint32_t *)&unitree_low_command, (sizeof(unitree_go::msg::dds_::LowCmd_) >> 2) - 1); lowcmd_publisher->Write(unitree_low_command); } @@ -160,8 +160,8 @@ void RL_Real::RunModel() if (this->running_state == STATE_RL_RUNNING) { this->obs.ang_vel = torch::tensor(this->robot_state.imu.gyroscope).unsqueeze(0); - // this->obs.commands = torch::tensor({{this->unitree_joy.ly, -this->unitree_joy.rx, -this->unitree_joy.lx}}); - this->obs.commands = torch::tensor({{this->control.x, this->control.y, this->control.yaw}}); + this->obs.commands = torch::tensor({{this->joystick.ly(), -this->joystick.rx(), -this->joystick.lx()}}); + //this->obs.commands = torch::tensor({{this->control.x, this->control.y, this->control.yaw}}); this->obs.base_quat = torch::tensor(this->robot_state.imu.quaternion).unsqueeze(0); this->obs.dof_pos = torch::tensor(this->robot_state.motor_state.q).narrow(0, 0, this->params.num_of_dofs).unsqueeze(0); this->obs.dof_vel = torch::tensor(this->robot_state.motor_state.dq).narrow(0, 0, this->params.num_of_dofs).unsqueeze(0); @@ -330,12 +330,13 @@ void RL_Real::LowStateMessageHandler(const void *message) unitree_low_state = *(unitree_go::msg::dds_::LowState_ *)message; } -void RL_Real::LowCmdwriteHandler() +void RL_Real::JoystickHandler(const void *message) { - // this->unitree_low_command.crc() = Crc32Core((uint32_t *)&unitree_low_command, (sizeof(unitree_go::msg::dds_::LowCmd_)>>2)-1); - // this->lowcmd_publisher->Write(unitree_low_command); + joystick = *(unitree_go::msg::dds_::WirelessController_ *)message; + unitree_joy.value = joystick.keys(); } + void signalHandler(int signum) { exit(0); @@ -343,6 +344,7 @@ void signalHandler(int signum) int main(int argc, char **argv) { + std::cout<<"000000"<