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: fix pd params

This commit is contained in:
fan-ziqi 2024-03-15 19:27:52 +08:00
parent 6a23e62a47
commit 220347de1d
1 changed files with 6 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/rl_sar/src/rl_real.cpp
View File

@ -48,6 +48,7 @@ void RL_Real::RobotControl()
_percent = _percent > 1 ? 1 : _percent; _percent = _percent > 1 ? 1 : _percent;
for(int i = 0; i < 12; ++i) for(int i = 0; i < 12; ++i)
{ {
cmd.motorCmd[i].mode = 0x0A;
cmd.motorCmd[i].q = (1 - _percent) * _startPos[i] + _percent * params.default_dof_pos[0][dof_mapping[i]].item<double>(); cmd.motorCmd[i].q = (1 - _percent) * _startPos[i] + _percent * params.default_dof_pos[0][dof_mapping[i]].item<double>();
cmd.motorCmd[i].dq = 0; cmd.motorCmd[i].dq = 0;
cmd.motorCmd[i].Kp = 50; cmd.motorCmd[i].Kp = 50;
@ -72,6 +73,7 @@ void RL_Real::RobotControl()
{ {
for(int i = 0; i < 12; ++i) for(int i = 0; i < 12; ++i)
{ {
cmd.motorCmd[i].mode = 0x0A;
cmd.motorCmd[i].q = params.default_dof_pos[0][dof_mapping[i]].item<double>(); cmd.motorCmd[i].q = params.default_dof_pos[0][dof_mapping[i]].item<double>();
cmd.motorCmd[i].dq = 0; cmd.motorCmd[i].dq = 0;
cmd.motorCmd[i].Kp = 50; cmd.motorCmd[i].Kp = 50;
@ -88,7 +90,7 @@ void RL_Real::RobotControl()
float torque = torques[0][dof_mapping[i]].item<double>(); float torque = torques[0][dof_mapping[i]].item<double>();
// if(torque > 5.0f) torque = 5.0f; // if(torque > 5.0f) torque = 5.0f;
// if(torque < -5.0f) torque = -5.0f; // if(torque < -5.0f) torque = -5.0f;
cmd.motorCmd[i].mode = 0x0A;
cmd.motorCmd[i].q = 0; cmd.motorCmd[i].q = 0;
cmd.motorCmd[i].dq = 0; cmd.motorCmd[i].dq = 0;
cmd.motorCmd[i].Kp = 0; cmd.motorCmd[i].Kp = 0;
@ -98,6 +100,7 @@ void RL_Real::RobotControl()
#else #else
for (int i = 0; i < 12; ++i) for (int i = 0; i < 12; ++i)
{ {
cmd.motorCmd[i].mode = 0x0A;
cmd.motorCmd[i].q = target_dof_pos[0][dof_mapping[i]].item<double>(); cmd.motorCmd[i].q = target_dof_pos[0][dof_mapping[i]].item<double>();
cmd.motorCmd[i].dq = 0; cmd.motorCmd[i].dq = 0;
cmd.motorCmd[i].Kp = 15; cmd.motorCmd[i].Kp = 15;
@ -130,8 +133,8 @@ RL_Real::RL_Real() : safe(LeggedType::A1), udp(LOWLEVEL)
this->params.num_observations = 45; this->params.num_observations = 45;
this->params.clip_obs = 100.0; this->params.clip_obs = 100.0;
this->params.clip_actions = 100.0; this->params.clip_actions = 100.0;
this->params.damping = 1.0; // TODO this->params.damping = 0.5;
this->params.stiffness = 5; // TODO this->params.stiffness = 20;
this->params.d_gains = torch::ones(12) * this->params.damping; this->params.d_gains = torch::ones(12) * this->params.damping;
this->params.p_gains = torch::ones(12) * this->params.stiffness; this->params.p_gains = torch::ones(12) * this->params.stiffness;
this->params.action_scale = 0.25; this->params.action_scale = 0.25;