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 joy control

This commit is contained in:
fan-ziqi 2024-03-18 14:53:17 +08:00
parent 74a98ea429
commit 96e2aa1452
2 changed files with 101 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
src/rl_sar/include/rl_real.hpp
View File

@ -16,10 +16,10 @@ using namespace UNITREE_LEGGED_SDK;
enum RobotState {
STATE_WAITING = 0,
STATE_POS_START,
STATE_POS_GETUP,
STATE_RL_INIT,
STATE_RL_START,
STATE_POS_STOP,
STATE_RL_RUNNING,
STATE_POS_GETDOWN,
};
class RL_Real : public RL
@ -52,10 +52,10 @@ public:
std::shared_ptr<LoopFunc> loop_rl;
std::shared_ptr<LoopFunc> loop_plot;
float start_percent = 0.0;
float stop_percent = 0.0;
float start_pos[12];
float stop_pos[12];
float getup_percent = 0.0;
float getdown_percent = 0.0;
float start_pos[12];
float now_pos[12];
int robot_state = STATE_WAITING;
@ -68,6 +68,14 @@ private:
std::vector<double> joint_velocities;
int dof_mapping[13] = {3, 4, 5, 0, 1, 2, 9, 10, 11, 6, 7, 8};
float Kp[13] = {20, 20, 20, //fr
20, 20, 20, //fl
20, 20, 20, //rr
20, 20, 20};//rl
float Kd[13] = {0.5, 0.5, 0.5,
0.5, 0.5, 0.5,
0.5, 0.5, 0.5,
0.5, 0.5, 0.5};
std::chrono::high_resolution_clock::time_point start_time;

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

@ -1,6 +1,6 @@
#include "../include/rl_real.hpp"
// #define PLOT
#define PLOT
RL_Real rl_sar;
@ -11,98 +11,117 @@ void RL_Real::RobotControl()
memcpy(&_keyData, state.wirelessRemote, 40);
// get joy button
if(robot_state < STATE_POS_START && (int)_keyData.btn.components.R2 == 1)
{
start_percent = 0.0;
for(int i = 0; i < 12; ++i)
{
start_pos[i] = state.motorState[i].q;
}
robot_state = STATE_POS_START;
}
else if(robot_state < STATE_RL_INIT && (int)_keyData.btn.components.R1 == 1)
{
robot_state = STATE_RL_INIT;
}
else if(robot_state == STATE_RL_START && (int)_keyData.btn.components.L2 == 1)
{
stop_percent = 0.0;
for(int i = 0; i < 12; ++i)
{
stop_pos[i] = state.motorState[i].q;
}
robot_state = STATE_POS_STOP;
}
// wait for standup
// waiting
if(robot_state == STATE_WAITING)
{
for(int i = 0; i < 12; ++i)
{
cmd.motorCmd[i].q = state.motorState[i].q;
}
}
// standup (position control)
else if(robot_state == STATE_POS_START && start_percent != 1)
{
start_percent += 1 / 1000.0;
start_percent = start_percent > 1 ? 1 : start_percent;
for(int i = 0; i < 12; ++i)
if((int)_keyData.btn.components.R2 == 1)
{
cmd.motorCmd[i].mode = 0x0A;
cmd.motorCmd[i].q = (1 - start_percent) * start_pos[i] + start_percent * params.default_dof_pos[0][dof_mapping[i]].item<double>();
cmd.motorCmd[i].dq = 0;
cmd.motorCmd[i].Kp = 50;
cmd.motorCmd[i].Kd = 3;
cmd.motorCmd[i].tau = 0;
getup_percent = 0.0;
for(int i = 0; i < 12; ++i)
{
now_pos[i] = state.motorState[i].q;
start_pos[i] = now_pos[i];
}
robot_state = STATE_POS_GETUP;
}
}
// stand up (position control)
else if(robot_state == STATE_POS_GETUP)
{
if(getup_percent != 1)
{
getup_percent += 1 / 1000.0;
getup_percent = getup_percent > 1 ? 1 : getup_percent;
for(int i = 0; i < 12; ++i)
{
cmd.motorCmd[i].mode = 0x0A;
cmd.motorCmd[i].q = (1 - getup_percent) * now_pos[i] + getup_percent * params.default_dof_pos[0][dof_mapping[i]].item<double>();
cmd.motorCmd[i].dq = 0;
cmd.motorCmd[i].Kp = 50;
cmd.motorCmd[i].Kd = 3;
cmd.motorCmd[i].tau = 0;
}
printf("getting up %.3f%%\r", getup_percent*100.0);
}
if((int)_keyData.btn.components.R1 == 1)
{
robot_state = STATE_RL_INIT;
}
else if((int)_keyData.btn.components.L2 == 1)
{
getdown_percent = 0.0;
for(int i = 0; i < 12; ++i)
{
now_pos[i] = state.motorState[i].q;
}
robot_state = STATE_POS_GETDOWN;
}
printf("starting %.3f%%\r", start_percent*100.0);
}
// init obs and start rl loop
else if(robot_state == STATE_RL_INIT && start_percent == 1)
else if(robot_state == STATE_RL_INIT)
{
robot_state = STATE_RL_START;
this->InitObservations();
printf("\nstart rl loop\n");
loop_rl->start();
if(getup_percent == 1)
{
robot_state = STATE_RL_RUNNING;
this->InitObservations();
printf("\nstart rl loop\n");
loop_rl->start();
}
}
// rl loop
else if(robot_state == STATE_RL_START)
else if(robot_state == STATE_RL_RUNNING)
{
for (int i = 0; i < 12; ++i)
for(int i = 0; i < 12; ++i)
{
cmd.motorCmd[i].mode = 0x0A;
cmd.motorCmd[i].q = output_dof_pos[0][dof_mapping[i]].item<double>();
cmd.motorCmd[i].dq = 0;
// cmd.motorCmd[i].Kp = Kp[dof_mapping[i]];
// cmd.motorCmd[i].Kd = Kd[dof_mapping[i]];
cmd.motorCmd[i].Kp = params.stiffness;
cmd.motorCmd[i].Kd = params.damping;
// cmd.motorCmd[i].tau = output_torques[0][dof_mapping[i]].item<double>();
cmd.motorCmd[i].tau = 0;
}
}
// move to start pos
else if(robot_state == STATE_POS_STOP && stop_percent != 1)
{
stop_percent += 1 / 1000.0;
stop_percent = stop_percent > 1 ? 1 : stop_percent;
for(int i = 0; i < 12; ++i)
if((int)_keyData.btn.components.L2 == 1)
{
cmd.motorCmd[i].mode = 0x0A;
cmd.motorCmd[i].q = (1 - stop_percent) * stop_pos[i] + stop_percent * start_pos[i];
cmd.motorCmd[i].dq = 0;
cmd.motorCmd[i].Kp = 50;
cmd.motorCmd[i].Kd = 3;
cmd.motorCmd[i].tau = 0;
getdown_percent = 0.0;
for(int i = 0; i < 12; ++i)
{
now_pos[i] = state.motorState[i].q;
}
robot_state = STATE_POS_GETDOWN;
}
printf("stopping %.3f%%\r", stop_percent*100.0);
}
else if(robot_state == STATE_POS_STOP && stop_percent == 1)
// get down (position control)
else if(robot_state == STATE_POS_GETDOWN)
{
robot_state = STATE_WAITING;
this->InitObservations();
printf("\nstop rl loop\n");
loop_rl->shutdown();
if(getdown_percent != 1)
{
getdown_percent += 1 / 1000.0;
getdown_percent = getdown_percent > 1 ? 1 : getdown_percent;
for(int i = 0; i < 12; ++i)
{
cmd.motorCmd[i].mode = 0x0A;
cmd.motorCmd[i].q = (1 - getdown_percent) * now_pos[i] + getdown_percent * start_pos[i];
cmd.motorCmd[i].dq = 0;
cmd.motorCmd[i].Kp = 50;
cmd.motorCmd[i].Kd = 3;
cmd.motorCmd[i].tau = 0;
}
printf("getting down %.3f%%\r", getdown_percent*100.0);
}
if(getdown_percent == 1)
{
robot_state = STATE_WAITING;
this->InitObservations();
printf("\nstop rl loop\n");
loop_rl->shutdown();
}
}
safe.PowerProtect(cmd, state, 7);
@ -205,7 +224,7 @@ void RL_Real::Plot()
void RL_Real::RunModel()
{
if(robot_state == STATE_RL_START)
if(robot_state == STATE_RL_RUNNING)
{
// auto duration = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_time).count();
// std::cout << "Execution time: " << duration << " microseconds" << std::endl;