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: update a1 init process

This commit is contained in:
fan-ziqi 2024-10-12 22:30:43 +08:00
parent dc8353a9ed
commit 640f3c58dc
1 changed files with 4 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/rl_sar/src/rl_real_a1.cpp
View File

@ -25,9 +25,10 @@ RL_Real::RL_Real() : unitree_safe(UNITREE_LEGGED_SDK::LeggedType::A1), unitree_u
this->history_obs_buf = ObservationBuffer(1, this->params.num_observations, this->params.observations_history.size()); this->history_obs_buf = ObservationBuffer(1, this->params.num_observations, this->params.observations_history.size());
} }
// init robot
this->unitree_udp.InitCmdData(this->unitree_low_command); this->unitree_udp.InitCmdData(this->unitree_low_command);
// init // init rl
torch::autograd::GradMode::set_enabled(false); torch::autograd::GradMode::set_enabled(false);
this->InitObservations(); this->InitObservations();
this->InitOutputs(); this->InitOutputs();
@ -79,6 +80,7 @@ RL_Real::~RL_Real()
void RL_Real::GetState(RobotState<double> *state) void RL_Real::GetState(RobotState<double> *state)
{ {
// TODO-devel-mutex
this->unitree_udp.GetRecv(this->unitree_low_state); this->unitree_udp.GetRecv(this->unitree_low_state);
memcpy(&this->unitree_joy, this->unitree_low_state.wirelessRemote, 40); memcpy(&this->unitree_joy, this->unitree_low_state.wirelessRemote, 40);
@ -154,6 +156,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->unitree_joy.ly, -this->unitree_joy.rx, -this->unitree_joy.lx}}); 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.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);