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 bug

This commit is contained in:
zma69650 2024-10-14 10:43:48 +08:00
parent 1671d80772
commit 194435355c
4 changed files with 73 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/rl_sar/CMakeLists.txt
View File

@ -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)

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

@ -8,6 +8,8 @@
#include <unitree/robot/channel/channel_subscriber.hpp>
#include <unitree/idl/go2/LowState_.hpp>
#include <unitree/idl/go2/LowCmd_.hpp>
#include <unitree/idl/go2/WirelessController_.hpp>
#include <unitree/robot/client/client.hpp>
#include <unitree/common/time/time_tool.hpp>
#include <unitree/common/thread/thread.hpp>
#include <unitree/robot/go2/robot_state/robot_state_client.hpp>
@ -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<unitree_go::msg::dds_::LowCmd_> lowcmd_publisher; // publisher
ChannelSubscriberPtr<unitree_go::msg::dds_::LowState_> lowstate_subscriber; //subscriber
// xRockerBtnDataStruct unitree_joy; // TODO-devel-go2 go2键盘接口
ChannelSubscriberPtr<unitree_go::msg::dds_::WirelessController_> joystick_subscriber;
xKeySwitchUnion unitree_joy;
// others
int motiontime = 0;

16
src/rl_sar/models/go2_isaacgym/config.yaml
View File

@ -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,

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

@ -5,6 +5,8 @@
RL_Real::RL_Real(const std::string &network_interface)
{
std::cout<<"000000"<<std::endl;
std::cout<<"000000"<<std::endl;
// read params from yaml
this->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"<<std::endl;
// init robot
ChannelFactory::Instance()->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<unitree_go::msg::dds_::LowState_>(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<unitree_go::msg::dds_::WirelessController_>(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<double> *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<double> *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"<<std::endl;
signal(SIGINT, signalHandler);
if (argc < 2)
@ -350,8 +352,8 @@ int main(int argc, char **argv)
std::cout << "Usage: " << argv[0] << " networkInterface" << std::endl;
exit(-1);
}
RL_Real rl_sar(argv[1]);
std::cout<<argv[1]<<std::endl;
RL_Real rl_sar(argv[0]);
while (1)
{