EmbodiedAI
/
rl_sar
mirror of https://github.com/fan-ziqi/rl_sar.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

fix: Segmentation fault

This commit is contained in:
fan-ziqi 2024-10-14 17:52:17 +08:00
parent 194435355c
commit 1c8211e16a
2 changed files with 63 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
src/rl_sar/include/rl_real_go2.hpp
View File

@ -13,7 +13,6 @@
#include <unitree/common/time/time_tool.hpp> #include <unitree/common/time/time_tool.hpp>
#include <unitree/common/thread/thread.hpp> #include <unitree/common/thread/thread.hpp>
#include <unitree/robot/go2/robot_state/robot_state_client.hpp> #include <unitree/robot/go2/robot_state/robot_state_client.hpp>
// #include "unitree_joystick.h" // TODO-devel-go2 go2键盘接口
#include <csignal> #include <csignal>
#include "matplotlibcpp.h" #include "matplotlibcpp.h"
@ -31,34 +30,32 @@ constexpr double VelStopF = (16000.0f);
// 遥控器键值联合体 // 遥控器键值联合体
typedef union typedef union
{ {
struct struct
{ {
uint8_t R1 : 1; uint8_t R1 : 1;
uint8_t L1 : 1; uint8_t L1 : 1;
uint8_t start : 1; uint8_t start : 1;
uint8_t select : 1; uint8_t select : 1;
uint8_t R2 : 1; uint8_t R2 : 1;
uint8_t L2 : 1; uint8_t L2 : 1;
uint8_t F1 : 1; uint8_t F1 : 1;
uint8_t F2 : 1; uint8_t F2 : 1;
uint8_t A : 1; uint8_t A : 1;
uint8_t B : 1; uint8_t B : 1;
uint8_t X : 1; uint8_t X : 1;
uint8_t Y : 1; uint8_t Y : 1;
uint8_t up : 1; uint8_t up : 1;
uint8_t right : 1; uint8_t right : 1;
uint8_t down : 1; uint8_t down : 1;
uint8_t left : 1; uint8_t left : 1;
} components; } components;
uint16_t value; uint16_t value;
} xKeySwitchUnion; } xKeySwitchUnion;
class RL_Real : public RL class RL_Real : public RL
{ {
public: public:
RL_Real(const std::string &network_interface); RL_Real();
~RL_Real(); ~RL_Real();
private: private:
@ -87,18 +84,17 @@ private:
// unitree interface // unitree interface
void InitRobotStateClient(); void InitRobotStateClient();
int QueryServiceStatus(const std::string &serviceName);
void ActivateService(const std::string &serviceName, int activate);
uint32_t Crc32Core(uint32_t *ptr, uint32_t len);
void InitLowCmd(); void InitLowCmd();
int QueryServiceStatus(const std::string &serviceName);
uint32_t Crc32Core(uint32_t *ptr, uint32_t len);
void LowStateMessageHandler(const void *messages); void LowStateMessageHandler(const void *messages);
void JoystickHandler(const void *message); void JoystickHandler(const void *message);
RobotStateClient rsc; RobotStateClient rsc;
unitree_go::msg::dds_::LowCmd_ unitree_low_command{}; // default init unitree_go::msg::dds_::LowCmd_ unitree_low_command{};
unitree_go::msg::dds_::LowState_ unitree_low_state{}; // default init unitree_go::msg::dds_::LowState_ unitree_low_state{};
unitree_go::msg::dds_::WirelessController_ joystick{}; unitree_go::msg::dds_::WirelessController_ joystick{};
ChannelPublisherPtr<unitree_go::msg::dds_::LowCmd_> lowcmd_publisher; // publisher ChannelPublisherPtr<unitree_go::msg::dds_::LowCmd_> lowcmd_publisher;
ChannelSubscriberPtr<unitree_go::msg::dds_::LowState_> lowstate_subscriber; //subscriber ChannelSubscriberPtr<unitree_go::msg::dds_::LowState_> lowstate_subscriber;
ChannelSubscriberPtr<unitree_go::msg::dds_::WirelessController_> joystick_subscriber; ChannelSubscriberPtr<unitree_go::msg::dds_::WirelessController_> joystick_subscriber;
xKeySwitchUnion unitree_joy; xKeySwitchUnion unitree_joy;

75
src/rl_sar/src/rl_real_go2.cpp
View File

@ -3,10 +3,8 @@
// #define PLOT // #define PLOT
// #define CSV_LOGGER // #define CSV_LOGGER
RL_Real::RL_Real(const std::string &network_interface) RL_Real::RL_Real()
{ {
std::cout<<"000000"<<std::endl;
std::cout<<"000000"<<std::endl;
// read params from yaml // read params from yaml
this->robot_name = "go2_isaacgym"; this->robot_name = "go2_isaacgym";
this->ReadYaml(this->robot_name); this->ReadYaml(this->robot_name);
@ -18,27 +16,25 @@ RL_Real::RL_Real(const std::string &network_interface)
observation = "ang_vel_body"; observation = "ang_vel_body";
} }
} }
std::cout<<"000000"<<std::endl;
// init robot // init robot
ChannelFactory::Instance()->Init(0, network_interface);
this->InitRobotStateClient(); this->InitRobotStateClient();
while (this->QueryServiceStatus("sport_mode")) while (this->QueryServiceStatus("sport_mode"))
{ {
std::cout << "Try to deactivate the service: " << "sport_mode" << std::endl; std::cout << "Try to deactivate the service: " << "sport_mode" << std::endl;
this->ActivateService("sport_mode", 0); this->rsc.ServiceSwitch("sport_mode", 0);
sleep(1); sleep(1);
} }
this->InitLowCmd(); this->InitLowCmd();
// create publisher // create publisher
lowcmd_publisher.reset(new ChannelPublisher<unitree_go::msg::dds_::LowCmd_>(TOPIC_LOWCMD)); this->lowcmd_publisher.reset(new ChannelPublisher<unitree_go::msg::dds_::LowCmd_>(TOPIC_LOWCMD));
lowcmd_publisher->InitChannel(); this->lowcmd_publisher->InitChannel();
// create subscriber // create subscriber
lowstate_subscriber.reset(new ChannelSubscriber<unitree_go::msg::dds_::LowState_>(TOPIC_LOWSTATE)); this->lowstate_subscriber.reset(new ChannelSubscriber<unitree_go::msg::dds_::LowState_>(TOPIC_LOWSTATE));
lowstate_subscriber->InitChannel(std::bind(&RL_Real::LowStateMessageHandler, this, std::placeholders::_1), 1); this->lowstate_subscriber->InitChannel(std::bind(&RL_Real::LowStateMessageHandler, this, std::placeholders::_1), 1);
joystick_subscriber.reset(new ChannelSubscriber<unitree_go::msg::dds_::WirelessController_>(TOPIC_JOYSTICK));
joystick_subscriber->InitChannel(std::bind(&RL_Real::JoystickHandler, this, std::placeholders::_1), 1);
this->joystick_subscriber.reset(new ChannelSubscriber<unitree_go::msg::dds_::WirelessController_>(TOPIC_JOYSTICK));
this->joystick_subscriber->InitChannel(std::bind(&RL_Real::JoystickHandler, this, std::placeholders::_1), 1);
// init rl // init rl
torch::autograd::GradMode::set_enabled(false); torch::autograd::GradMode::set_enabled(false);
@ -161,7 +157,7 @@ void RL_Real::RunModel()
{ {
this->obs.ang_vel = torch::tensor(this->robot_state.imu.gyroscope).unsqueeze(0); this->obs.ang_vel = torch::tensor(this->robot_state.imu.gyroscope).unsqueeze(0);
this->obs.commands = torch::tensor({{this->joystick.ly(), -this->joystick.rx(), -this->joystick.lx()}}); 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.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.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_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); this->obs.dof_vel = torch::tensor(this->robot_state.motor_state.dq).narrow(0, 0, this->params.num_of_dofs).unsqueeze(0);
@ -245,7 +241,7 @@ uint32_t RL_Real::Crc32Core(uint32_t *ptr, uint32_t len)
unsigned int CRC32 = 0xFFFFFFFF; unsigned int CRC32 = 0xFFFFFFFF;
const unsigned int dwPolynomial = 0x04c11db7; const unsigned int dwPolynomial = 0x04c11db7;
for (unsigned int i = 0; i < len; i++) for (unsigned int i = 0; i < len; ++i)
{ {
xbit = 1 << 31; xbit = 1 << 31;
data = ptr[i]; data = ptr[i];
@ -262,7 +258,9 @@ uint32_t RL_Real::Crc32Core(uint32_t *ptr, uint32_t len)
} }
if (data & xbit) if (data & xbit)
{
CRC32 ^= dwPolynomial; CRC32 ^= dwPolynomial;
}
xbit >>= 1; xbit >>= 1;
} }
} }
@ -272,35 +270,35 @@ uint32_t RL_Real::Crc32Core(uint32_t *ptr, uint32_t len)
void RL_Real::InitLowCmd() void RL_Real::InitLowCmd()
{ {
unitree_low_command.head()[0] = 0xFE; this->unitree_low_command.head()[0] = 0xFE;
unitree_low_command.head()[1] = 0xEF; this->unitree_low_command.head()[1] = 0xEF;
unitree_low_command.level_flag() = 0xFF; this->unitree_low_command.level_flag() = 0xFF;
unitree_low_command.gpio() = 0; this->unitree_low_command.gpio() = 0;
for (int i = 0; i < 20; i++) for (int i = 0; i < 20; ++i)
{ {
unitree_low_command.motor_cmd()[i].mode() = (0x01); // motor switch to servo (PMSM) mode this->unitree_low_command.motor_cmd()[i].mode() = (0x01); // motor switch to servo (PMSM) mode
unitree_low_command.motor_cmd()[i].q() = (PosStopF); this->unitree_low_command.motor_cmd()[i].q() = (PosStopF);
unitree_low_command.motor_cmd()[i].kp() = (0); this->unitree_low_command.motor_cmd()[i].kp() = (0);
unitree_low_command.motor_cmd()[i].dq() = (VelStopF); this->unitree_low_command.motor_cmd()[i].dq() = (VelStopF);
unitree_low_command.motor_cmd()[i].kd() = (0); this->unitree_low_command.motor_cmd()[i].kd() = (0);
unitree_low_command.motor_cmd()[i].tau() = (0); this->unitree_low_command.motor_cmd()[i].tau() = (0);
} }
} }
void RL_Real::InitRobotStateClient() void RL_Real::InitRobotStateClient()
{ {
rsc.SetTimeout(10.0f); this->rsc.SetTimeout(10.0f);
rsc.Init(); this->rsc.Init();
} }
int RL_Real::QueryServiceStatus(const std::string &serviceName) int RL_Real::QueryServiceStatus(const std::string &serviceName)
{ {
std::vector<ServiceState> serviceStateList; std::vector<ServiceState> serviceStateList;
int ret, serviceStatus; int ret, serviceStatus;
ret = rsc.ServiceList(serviceStateList); ret = this->rsc.ServiceList(serviceStateList);
size_t i, count = serviceStateList.size(); size_t i, count = serviceStateList.size();
for (i = 0; i < count; i++) for (i = 0; i < count; ++i)
{ {
const ServiceState &serviceState = serviceStateList[i]; const ServiceState &serviceState = serviceStateList[i];
if (serviceState.name == serviceName) if (serviceState.name == serviceName)
@ -320,23 +318,17 @@ int RL_Real::QueryServiceStatus(const std::string &serviceName)
return serviceStatus; return serviceStatus;
} }
void RL_Real::ActivateService(const std::string &serviceName, int activate)
{
rsc.ServiceSwitch(serviceName, activate);
}
void RL_Real::LowStateMessageHandler(const void *message) void RL_Real::LowStateMessageHandler(const void *message)
{ {
unitree_low_state = *(unitree_go::msg::dds_::LowState_ *)message; this->unitree_low_state = *(unitree_go::msg::dds_::LowState_ *)message;
} }
void RL_Real::JoystickHandler(const void *message) void RL_Real::JoystickHandler(const void *message)
{ {
joystick = *(unitree_go::msg::dds_::WirelessController_ *)message; joystick = *(unitree_go::msg::dds_::WirelessController_ *)message;
unitree_joy.value = joystick.keys(); this->unitree_joy.value = joystick.keys();
} }
void signalHandler(int signum) void signalHandler(int signum)
{ {
exit(0); exit(0);
@ -344,7 +336,6 @@ void signalHandler(int signum)
int main(int argc, char **argv) int main(int argc, char **argv)
{ {
std::cout<<"000000"<<std::endl;
signal(SIGINT, signalHandler); signal(SIGINT, signalHandler);
if (argc < 2) if (argc < 2)
@ -352,8 +343,10 @@ int main(int argc, char **argv)
std::cout << "Usage: " << argv[0] << " networkInterface" << std::endl; std::cout << "Usage: " << argv[0] << " networkInterface" << std::endl;
exit(-1); exit(-1);
} }
std::cout<<argv[1]<<std::endl;
RL_Real rl_sar(argv[0]); ChannelFactory::Instance()->Init(0, argv[1]);
RL_Real rl_sar;
while (1) while (1)
{ {