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 { enum RobotState {
STATE_WAITING = 0, STATE_WAITING = 0,
STATE_POS_START, STATE_POS_GETUP,
STATE_RL_INIT, STATE_RL_INIT,
STATE_RL_START, STATE_RL_RUNNING,
STATE_POS_STOP, STATE_POS_GETDOWN,
}; };
class RL_Real : public RL class RL_Real : public RL
@ -52,10 +52,10 @@ public:
std::shared_ptr<LoopFunc> loop_rl; std::shared_ptr<LoopFunc> loop_rl;
std::shared_ptr<LoopFunc> loop_plot; std::shared_ptr<LoopFunc> loop_plot;
float start_percent = 0.0; float getup_percent = 0.0;
float stop_percent = 0.0; float getdown_percent = 0.0;
float start_pos[12]; float start_pos[12];
float stop_pos[12]; float now_pos[12];
int robot_state = STATE_WAITING; int robot_state = STATE_WAITING;
@ -68,6 +68,14 @@ private:
std::vector<double> joint_velocities; std::vector<double> joint_velocities;
int dof_mapping[13] = {3, 4, 5, 0, 1, 2, 9, 10, 11, 6, 7, 8}; 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; 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" #include "../include/rl_real.hpp"
// #define PLOT #define PLOT
RL_Real rl_sar; RL_Real rl_sar;
@ -11,98 +11,117 @@ void RL_Real::RobotControl()
memcpy(&_keyData, state.wirelessRemote, 40); memcpy(&_keyData, state.wirelessRemote, 40);
// get joy button // waiting
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
if(robot_state == STATE_WAITING) if(robot_state == STATE_WAITING)
{ {
for(int i = 0; i < 12; ++i) for(int i = 0; i < 12; ++i)
{ {
cmd.motorCmd[i].q = state.motorState[i].q; cmd.motorCmd[i].q = state.motorState[i].q;
} }
} if((int)_keyData.btn.components.R2 == 1)
// 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)
{ {
cmd.motorCmd[i].mode = 0x0A; getup_percent = 0.0;
cmd.motorCmd[i].q = (1 - start_percent) * start_pos[i] + start_percent * params.default_dof_pos[0][dof_mapping[i]].item<double>(); for(int i = 0; i < 12; ++i)
cmd.motorCmd[i].dq = 0; {
cmd.motorCmd[i].Kp = 50; now_pos[i] = state.motorState[i].q;
cmd.motorCmd[i].Kd = 3; start_pos[i] = now_pos[i];
cmd.motorCmd[i].tau = 0; }
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 // 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; if(getup_percent == 1)
this->InitObservations(); {
printf("\nstart rl loop\n"); robot_state = STATE_RL_RUNNING;
loop_rl->start(); this->InitObservations();
printf("\nstart rl loop\n");
loop_rl->start();
}
} }
// rl loop // 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].mode = 0x0A;
cmd.motorCmd[i].q = output_dof_pos[0][dof_mapping[i]].item<double>(); cmd.motorCmd[i].q = output_dof_pos[0][dof_mapping[i]].item<double>();
cmd.motorCmd[i].dq = 0; 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].Kp = params.stiffness;
cmd.motorCmd[i].Kd = params.damping; cmd.motorCmd[i].Kd = params.damping;
// cmd.motorCmd[i].tau = output_torques[0][dof_mapping[i]].item<double>(); // cmd.motorCmd[i].tau = output_torques[0][dof_mapping[i]].item<double>();
cmd.motorCmd[i].tau = 0; cmd.motorCmd[i].tau = 0;
} }
} if((int)_keyData.btn.components.L2 == 1)
// 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)
{ {
cmd.motorCmd[i].mode = 0x0A; getdown_percent = 0.0;
cmd.motorCmd[i].q = (1 - stop_percent) * stop_pos[i] + stop_percent * start_pos[i]; for(int i = 0; i < 12; ++i)
cmd.motorCmd[i].dq = 0; {
cmd.motorCmd[i].Kp = 50; now_pos[i] = state.motorState[i].q;
cmd.motorCmd[i].Kd = 3; }
cmd.motorCmd[i].tau = 0; 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; if(getdown_percent != 1)
this->InitObservations(); {
printf("\nstop rl loop\n"); getdown_percent += 1 / 1000.0;
loop_rl->shutdown(); 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); safe.PowerProtect(cmd, state, 7);
@ -205,7 +224,7 @@ void RL_Real::Plot()
void RL_Real::RunModel() 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(); // 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; // std::cout << "Execution time: " << duration << " microseconds" << std::endl;