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

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

@ -13,7 +13,6 @@
#include <unitree/common/time/time_tool.hpp>
#include <unitree/common/thread/thread.hpp>
#include <unitree/robot/go2/robot_state/robot_state_client.hpp>
// #include "unitree_joystick.h" // TODO-devel-go2 go2键盘接口
#include <csignal>
#include "matplotlibcpp.h"
@ -31,34 +30,32 @@ 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;
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:
RL_Real(const std::string &network_interface);
RL_Real();
~RL_Real();
private:
@ -87,20 +84,19 @@ private:
// unitree interface
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();
int QueryServiceStatus(const std::string &serviceName);
uint32_t Crc32Core(uint32_t *ptr, uint32_t len);
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_::LowCmd_ unitree_low_command{};
unitree_go::msg::dds_::LowState_ unitree_low_state{};
unitree_go::msg::dds_::WirelessController_ joystick{};
ChannelPublisherPtr<unitree_go::msg::dds_::LowCmd_> lowcmd_publisher; // publisher
ChannelSubscriberPtr<unitree_go::msg::dds_::LowState_> lowstate_subscriber; //subscriber
ChannelSubscriberPtr<unitree_go::msg::dds_::WirelessController_> joystick_subscriber;
xKeySwitchUnion unitree_joy;
ChannelPublisherPtr<unitree_go::msg::dds_::LowCmd_> lowcmd_publisher;
ChannelSubscriberPtr<unitree_go::msg::dds_::LowState_> lowstate_subscriber;
ChannelSubscriberPtr<unitree_go::msg::dds_::WirelessController_> joystick_subscriber;
xKeySwitchUnion unitree_joy;
// others
int motiontime = 0;

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

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