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: make up/down static

This commit is contained in:
fan-ziqi 2024-05-29 12:27:09 +08:00
parent 0740bd6e42
commit be79e7f64a
4 changed files with 11 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/rl_sar/library/rl_sdk/rl_sdk.cpp
View File

@ -82,6 +82,11 @@ torch::Tensor RL::QuatRotateInverse(torch::Tensor q, torch::Tensor v)
void RL::StateController(const RobotState<double> *state, RobotCommand<double> *command) void RL::StateController(const RobotState<double> *state, RobotCommand<double> *command)
{ {
static RobotState<double> start_state;
static RobotState<double> now_state;
static float getup_percent = 0.0;
static float getdown_percent = 0.0;
// waiting // waiting
if(running_state == STATE_WAITING) if(running_state == STATE_WAITING)
{ {
@ -95,8 +100,8 @@ void RL::StateController(const RobotState<double> *state, RobotCommand<double> *
getup_percent = 0.0; getup_percent = 0.0;
for(int i = 0; i < params.num_of_dofs; ++i) for(int i = 0; i < params.num_of_dofs; ++i)
{ {
now_pos[i] = state->motor_state.q[i]; now_state.motor_state.q[i] = state->motor_state.q[i];
start_pos[i] = now_pos[i]; start_state.motor_state.q[i] = now_state.motor_state.q[i];
} }
running_state = STATE_POS_GETUP; running_state = STATE_POS_GETUP;
} }
@ -110,7 +115,7 @@ void RL::StateController(const RobotState<double> *state, RobotCommand<double> *
getup_percent = getup_percent > 1.0 ? 1.0 : getup_percent; getup_percent = getup_percent > 1.0 ? 1.0 : getup_percent;
for(int i = 0; i < params.num_of_dofs; ++i) for(int i = 0; i < params.num_of_dofs; ++i)
{ {
command->motor_command.q[i] = (1 - getup_percent) * now_pos[i] + getup_percent * params.default_dof_pos[0][i].item<double>(); command->motor_command.q[i] = (1 - getup_percent) * now_state.motor_state.q[i] + getup_percent * params.default_dof_pos[0][i].item<double>();
command->motor_command.dq[i] = 0; command->motor_command.dq[i] = 0;
command->motor_command.kp[i] = params.fixed_kp[0][i].item<double>(); command->motor_command.kp[i] = params.fixed_kp[0][i].item<double>();
command->motor_command.kd[i] = params.fixed_kd[0][i].item<double>(); command->motor_command.kd[i] = params.fixed_kd[0][i].item<double>();
@ -130,7 +135,7 @@ void RL::StateController(const RobotState<double> *state, RobotCommand<double> *
getdown_percent = 0.0; getdown_percent = 0.0;
for(int i = 0; i < params.num_of_dofs; ++i) for(int i = 0; i < params.num_of_dofs; ++i)
{ {
now_pos[i] = state->motor_state.q[i]; now_state.motor_state.q[i] = state->motor_state.q[i];
} }
running_state = STATE_POS_GETDOWN; running_state = STATE_POS_GETDOWN;
} }
@ -163,7 +168,7 @@ void RL::StateController(const RobotState<double> *state, RobotCommand<double> *
getdown_percent = 0.0; getdown_percent = 0.0;
for(int i = 0; i < params.num_of_dofs; ++i) for(int i = 0; i < params.num_of_dofs; ++i)
{ {
now_pos[i] = state->motor_state.q[i]; now_state.motor_state.q[i] = state->motor_state.q[i];
} }
running_state = STATE_POS_GETDOWN; running_state = STATE_POS_GETDOWN;
} }
@ -177,7 +182,7 @@ void RL::StateController(const RobotState<double> *state, RobotCommand<double> *
getdown_percent = getdown_percent > 1.0 ? 1.0 : getdown_percent; getdown_percent = getdown_percent > 1.0 ? 1.0 : getdown_percent;
for(int i = 0; i < params.num_of_dofs; ++i) for(int i = 0; i < params.num_of_dofs; ++i)
{ {
command->motor_command.q[i] = (1 - getdown_percent) * now_pos[i] + getdown_percent * start_pos[i]; command->motor_command.q[i] = (1 - getdown_percent) * now_state.motor_state.q[i] + getdown_percent * start_state.motor_state.q[i];
command->motor_command.dq[i] = 0; command->motor_command.dq[i] = 0;
command->motor_command.kp[i] = params.fixed_kp[0][i].item<double>(); command->motor_command.kp[i] = params.fixed_kp[0][i].item<double>();
command->motor_command.kd[i] = params.fixed_kd[0][i].item<double>(); command->motor_command.kd[i] = params.fixed_kd[0][i].item<double>();

5
src/rl_sar/library/rl_sdk/rl_sdk.hpp
View File

@ -157,11 +157,6 @@ protected:
// output buffer // output buffer
torch::Tensor output_torques; torch::Tensor output_torques;
torch::Tensor output_dof_pos; torch::Tensor output_dof_pos;
// getup getdown buffer
float getup_percent = 0.0;
float getdown_percent = 0.0;
std::vector<double> start_pos;
std::vector<double> now_pos;
}; };
#endif #endif

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

@ -18,8 +18,6 @@ RL_Real::RL_Real() : unitree_safe(UNITREE_LEGGED_SDK::LeggedType::A1), unitree_u
// init // init
torch::autograd::GradMode::set_enabled(false); torch::autograd::GradMode::set_enabled(false);
start_pos.resize(params.num_of_dofs);
now_pos.resize(params.num_of_dofs);
this->InitObservations(); this->InitObservations();
this->InitOutputs(); this->InitOutputs();
this->InitControl(); this->InitControl();

2
src/rl_sar/src/rl_sim.cpp
View File

@ -33,8 +33,6 @@ RL_Sim::RL_Sim()
// init // init
torch::autograd::GradMode::set_enabled(false); torch::autograd::GradMode::set_enabled(false);
motor_commands.resize(params.num_of_dofs); motor_commands.resize(params.num_of_dofs);
start_pos.resize(params.num_of_dofs);
now_pos.resize(params.num_of_dofs);
this->InitObservations(); this->InitObservations();
this->InitOutputs(); this->InitOutputs();
this->InitControl(); this->InitControl();