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

test go2

This commit is contained in:
zma69650 2024-10-15 09:00:53 +08:00
parent 194435355c
commit f025753371
2 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -96,7 +96,7 @@ private:
RobotStateClient rsc; RobotStateClient rsc;
unitree_go::msg::dds_::LowCmd_ unitree_low_command{}; // default init unitree_go::msg::dds_::LowCmd_ unitree_low_command{}; // default init
unitree_go::msg::dds_::LowState_ unitree_low_state{}; // default init unitree_go::msg::dds_::LowState_ unitree_low_state{}; // default init
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; // publisher
ChannelSubscriberPtr<unitree_go::msg::dds_::LowState_> lowstate_subscriber; //subscriber ChannelSubscriberPtr<unitree_go::msg::dds_::LowState_> lowstate_subscriber; //subscriber
ChannelSubscriberPtr<unitree_go::msg::dds_::WirelessController_> joystick_subscriber; ChannelSubscriberPtr<unitree_go::msg::dds_::WirelessController_> joystick_subscriber;

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

@ -36,8 +36,8 @@ RL_Real::RL_Real(const std::string &network_interface)
lowstate_subscriber.reset(new ChannelSubscriber<unitree_go::msg::dds_::LowState_>(TOPIC_LOWSTATE)); 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); 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.reset(new ChannelSubscriber<unitree_go::msg::dds_::WirelessController_>(TOPIC_JOYSTICK));
joystick_subscriber->InitChannel(std::bind(&RL_Real::JoystickHandler, this, std::placeholders::_1), 1); // joystick_subscriber->InitChannel(std::bind(&RL_Real::JoystickHandler, this, std::placeholders::_1), 1);
// init rl // init rl
@ -160,8 +160,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);
@ -332,8 +332,8 @@ void RL_Real::LowStateMessageHandler(const void *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(); // unitree_joy.value = joystick.keys();
} }