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

Merge branch 'master' of https://github.com/zma69650/rl_sar

This commit is contained in:
zma69650 2024-10-15 09:17:28 +08:00
parent f025753371 dfc77793c9
commit 2ee1ee50b3
4 changed files with 72 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
README.md
View File

@ -140,7 +140,9 @@ TODO-devel-go2 完善go2的使用说明
### Train the actuator network ### Train the actuator network
1. Uncomment `#define CSV_LOGGER` in the top of `rl_real.cpp`. You can also modify the corresponding part in the simulation program to collect simulation data for testing the training process. Take A1 as an example below
1. Uncomment `#define CSV_LOGGER` in the top of `rl_real_a1.cpp`. You can also modify the corresponding part in the simulation program to collect simulation data for testing the training process.
2. Run the control program, and the program will log all data after execution. 2. Run the control program, and the program will log all data after execution.
3. Stop the control program and start training the actuator network. Note that `rl_sar/src/rl_sar/models/` is omitted before the following paths. 3. Stop the control program and start training the actuator network. Note that `rl_sar/src/rl_sar/models/` is omitted before the following paths.
```bash ```bash
@ -162,10 +164,6 @@ In the following text, `<ROBOT>` represents the name of the robot
5. If you need to run simulations, modify the launch files as needed by referring to those in the `rl_sar/src/rl_sar/launch` directory. 5. If you need to run simulations, modify the launch files as needed by referring to those in the `rl_sar/src/rl_sar/launch` directory.
6. If you need to run on the physical robot, modify the file `rl_sar/src/rl_sar/src/rl_real_a1.cpp` as needed. 6. If you need to run on the physical robot, modify the file `rl_sar/src/rl_sar/src/rl_real_a1.cpp` as needed.
## Reference
[unitree_ros](https://github.com/unitreerobotics/unitree_ros)
## Contributing ## Contributing
Wholeheartedly welcome contributions from the community to make this framework mature and useful for everyone. These may happen as bug reports, feature requests, or code contributions. Wholeheartedly welcome contributions from the community to make this framework mature and useful for everyone. These may happen as bug reports, feature requests, or code contributions.

8
README_CN.md
View File

@ -141,7 +141,9 @@ TODO-devel-go2 完善go2的使用说明
### 训练执行器网络 ### 训练执行器网络
1. 取消注释`rl_real.cpp`中最上面的`#define CSV_LOGGER`，你也可以在仿真程序中修改对应部分采集仿真数据用来测试训练过程。 下面拿A1举例
1. 取消注释`rl_real_a1.cpp`中最上面的`#define CSV_LOGGER`，你也可以在仿真程序中修改对应部分采集仿真数据用来测试训练过程。
2. 运行控制程序，程序会在执行后记录所有数据。 2. 运行控制程序，程序会在执行后记录所有数据。
3. 停止控制程序，开始训练执行器网络。注意，下面的路径前均省略了`rl_sar/src/rl_sar/models/`。 3. 停止控制程序，开始训练执行器网络。注意，下面的路径前均省略了`rl_sar/src/rl_sar/models/`。
```bash ```bash
@ -163,10 +165,6 @@ TODO-devel-go2 完善go2的使用说明
5. 若需要运行仿真，则参考`rl_sar/src/rl_sar/launch`路径下的launch文件自行修改 5. 若需要运行仿真，则参考`rl_sar/src/rl_sar/launch`路径下的launch文件自行修改
6. 若需要运行实物，则参考`rl_sar/src/rl_sar/src/rl_real_a1.cpp`文件自行修改 6. 若需要运行实物，则参考`rl_sar/src/rl_sar/src/rl_real_a1.cpp`文件自行修改
## 参考
[unitree_ros](https://github.com/unitreerobotics/unitree_ros)
## 贡献 ## 贡献
衷心欢迎社区的贡献，以使这个框架更加成熟和对所有人有用。贡献可以是bug报告、功能请求或代码贡献。 衷心欢迎社区的贡献，以使这个框架更加成熟和对所有人有用。贡献可以是bug报告、功能请求或代码贡献。

20
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"
@ -53,12 +52,10 @@ typedef union
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;

79
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);
@ -160,8 +156,8 @@ void RL_Real::RunModel()
if (this->running_state == STATE_RL_RUNNING) if (this->running_state == STATE_RL_RUNNING)
{ {
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)
{ {