From 665c5718c17adc4292828288055a715af717c1c0 Mon Sep 17 00:00:00 2001 From: fan-ziqi Date: Wed, 12 Jun 2024 16:01:17 +0800 Subject: [PATCH] feat: change interaction logic --- README.md | 8 +++--- README_CN.md | 7 ++++-- src/rl_sar/include/rl_sim.hpp | 2 ++ src/rl_sar/launch/gazebo_a1.launch | 2 +- src/rl_sar/launch/gazebo_gr1t1.launch | 2 +- src/rl_sar/library/rl_sdk/rl_sdk.cpp | 8 +++--- src/rl_sar/library/rl_sdk/rl_sdk.hpp | 4 ++- src/rl_sar/src/rl_sim.cpp | 35 ++++++++++++++++++++------- 8 files changed, 47 insertions(+), 21 deletions(-) diff --git a/README.md b/README.md index e5fc364..20371d8 100644 --- a/README.md +++ b/README.md @@ -84,9 +84,11 @@ roslaunch rl_sar gazebo_.launch Where \ can be `a1` or `gr1t1`. -Press **0** on the keyboard to switch the robot to the default standing position, press **P** to switch to RL control mode, and press **1** in any state to switch to the initial lying position. WS controls x-axis, AD controls yaw, and JL controls y-axis. - -Press **R** to reset Gazebo environment. +Control: +* Press **\** to toggle simulation start/stop. +* **W** and **S** controls x-axis, **A** and **D** controls yaw, and **J** and **L** controls y-axis. +* Press **\** to sets all control commands to zero. +* If robot falls down, press **R** to reset Gazebo environment. ### Physical Robots diff --git a/README_CN.md b/README_CN.md index 38e5fb6..d76362c 100644 --- a/README_CN.md +++ b/README_CN.md @@ -84,9 +84,12 @@ roslaunch rl_sar gazebo_.launch 其中 \ 可以是 `a1` 或 `gr1t1`. -按下键盘上的**0**键让机器人切换到默认站起姿态，按下**P**键切换到RL控制模式，任意状态按下**1**键切换到最初的趴下姿态。WS控制x，AD控制yaw，JL控制y。 +控制： -按**R**重置Gazebo仿真环境。 +* 按 **\** 切换仿真器运行/停止。 +* **W** 和 **S** 控制x轴，**A** 和 **D** 控制yaw轴，**J** 和 **L** 控制y轴，按下空格重置控制指令。 +* 按 **\** 将所有控制指令设置为零。 +* 如果机器人摔倒，按 **R** 重置Gazebo环境。 ### 实物 diff --git a/src/rl_sar/include/rl_sim.hpp b/src/rl_sar/include/rl_sim.hpp index d39298c..b6512a9 100644 --- a/src/rl_sar/include/rl_sim.hpp +++ b/src/rl_sar/include/rl_sim.hpp @@ -56,6 +56,8 @@ private: ros::Subscriber joint_state_subscriber; ros::Subscriber cmd_vel_subscriber; ros::ServiceClient gazebo_set_model_state_client; + ros::ServiceClient gazebo_pause_physics_client; + ros::ServiceClient gazebo_unpause_physics_client; std::map joint_publishers; std::vector joint_publishers_commands; void ModelStatesCallback(const gazebo_msgs::ModelStates::ConstPtr &msg); diff --git a/src/rl_sar/launch/gazebo_a1.launch b/src/rl_sar/launch/gazebo_a1.launch index 9fa2b4f..32886be 100644 --- a/src/rl_sar/launch/gazebo_a1.launch +++ b/src/rl_sar/launch/gazebo_a1.launch @@ -32,7 +32,7 @@ + args="-urdf -z 0.6 -model $(arg rname)_gazebo -param robot_description"/> diff --git a/src/rl_sar/launch/gazebo_gr1t1.launch b/src/rl_sar/launch/gazebo_gr1t1.launch index 46a76dc..abf525c 100644 --- a/src/rl_sar/launch/gazebo_gr1t1.launch +++ b/src/rl_sar/launch/gazebo_gr1t1.launch @@ -28,7 +28,7 @@ + args="-urdf -z 1.0 -model $(arg rname)_gazebo -param robot_description"/> diff --git a/src/rl_sar/library/rl_sdk/rl_sdk.cpp b/src/rl_sar/library/rl_sdk/rl_sdk.cpp index 0841a34..d14ebb9 100644 --- a/src/rl_sar/library/rl_sdk/rl_sdk.cpp +++ b/src/rl_sar/library/rl_sdk/rl_sdk.cpp @@ -122,7 +122,7 @@ void RL::StateController(const RobotState *state, RobotCommand * command->motor_command.kd[i] = this->params.fixed_kd[0][i].item(); command->motor_command.tau[i] = 0; } - std::cout << LOGGER::INFO << "Getting up " << std::fixed << std::setprecision(2) << getup_percent * 100.0 << "%\r"; + std::cout << "\r" << std::flush << LOGGER::INFO << "Getting up " << std::fixed << std::setprecision(2) << getup_percent * 100.0 << std::flush; } if(this->control.control_state == STATE_RL_INIT) { @@ -157,8 +157,7 @@ void RL::StateController(const RobotState *state, RobotCommand * // rl loop else if(this->running_state == STATE_RL_RUNNING) { - std::cout << LOGGER::INFO << "RL Controller x:" << this->control.x << " y:" << this->control.y << " yaw:" << this->control.yaw << " \r"; - + std::cout << "\r" << std::flush << LOGGER::INFO << "RL Controller x:" << this->control.x << " y:" << this->control.y << " yaw:" << this->control.yaw << std::flush; for(int i = 0; i < this->params.num_of_dofs; ++i) { command->motor_command.q[i] = this->output_dof_pos[0][i].item(); @@ -205,7 +204,7 @@ void RL::StateController(const RobotState *state, RobotCommand * command->motor_command.kd[i] = this->params.fixed_kd[0][i].item(); command->motor_command.tau[i] = 0; } - std::cout << LOGGER::INFO << "Getting down " << std::fixed << std::setprecision(2) << getdown_percent * 100.0 << "%\r"; + std::cout << "\r" << std::flush << LOGGER::INFO << "Getting down " << std::fixed << std::setprecision(2) << getdown_percent * 100.0 << std::flush; } if(getdown_percent == 1) { @@ -291,6 +290,7 @@ void RL::KeyboardInterface() case 'l': this->control.y -= 0.1; break; case ' ': this->control.x = 0; this->control.y = 0; this->control.yaw = 0; break; case 'r': this->control.control_state = STATE_RESET_SIMULATION; break; + case '\n': this->control.control_state = STATE_TOGGLE_SIMULATION; break; default: break; } } diff --git a/src/rl_sar/library/rl_sdk/rl_sdk.hpp b/src/rl_sar/library/rl_sdk/rl_sdk.hpp index 2067265..37761a2 100644 --- a/src/rl_sar/library/rl_sdk/rl_sdk.hpp +++ b/src/rl_sar/library/rl_sdk/rl_sdk.hpp @@ -56,6 +56,7 @@ enum STATE { STATE_RL_RUNNING, STATE_POS_GETDOWN, STATE_RESET_SIMULATION, + STATE_TOGGLE_SIMULATION, }; struct Control @@ -148,7 +149,8 @@ public: // others std::string robot_name; - STATE running_state = STATE_WAITING; + STATE running_state = STATE_RL_RUNNING; // default running_state set to STATE_RL_RUNNING + bool simulation_running = false; // protect func void TorqueProtect(torch::Tensor origin_output_torques); diff --git a/src/rl_sar/src/rl_sim.cpp b/src/rl_sar/src/rl_sim.cpp index aab1ea3..fa9ad0f 100644 --- a/src/rl_sar/src/rl_sim.cpp +++ b/src/rl_sar/src/rl_sim.cpp @@ -57,7 +57,9 @@ RL_Sim::RL_Sim() // service this->gazebo_set_model_state_client = nh.serviceClient("/gazebo/set_model_state"); - + this->gazebo_pause_physics_client = nh.serviceClient("/gazebo/pause_physics"); + this->gazebo_unpause_physics_client = nh.serviceClient("/gazebo/unpause_physics"); + // loop this->loop_keyboard = std::make_shared("loop_keyboard", 0.05, std::bind(&RL_Sim::KeyboardInterface, this)); this->loop_control = std::make_shared("loop_control", this->params.dt, std::bind(&RL_Sim::RobotControl, this)); @@ -131,8 +133,6 @@ void RL_Sim::SetCommand(const RobotCommand *command) void RL_Sim::RobotControl() { - this->motiontime++; - if(this->control.control_state == STATE_RESET_SIMULATION) { gazebo_msgs::SetModelState set_model_state; @@ -144,10 +144,27 @@ void RL_Sim::RobotControl() this->control.control_state = STATE_WAITING; } - - this->GetState(&this->robot_state); - this->StateController(&this->robot_state, &this->robot_command); - this->SetCommand(&this->robot_command); + if(this->control.control_state == STATE_TOGGLE_SIMULATION) + { + std_srvs::Empty empty; + if(simulation_running) + { + this->gazebo_pause_physics_client.call(empty); + } + else + { + this->gazebo_unpause_physics_client.call(empty); + } + simulation_running = !simulation_running; + this->control.control_state = STATE_WAITING; + } + if(simulation_running) + { + this->motiontime++; + this->GetState(&this->robot_state); + this->StateController(&this->robot_state, &this->robot_command); + this->SetCommand(&this->robot_command); + } } void RL_Sim::ModelStatesCallback(const gazebo_msgs::ModelStates::ConstPtr &msg) @@ -178,7 +195,7 @@ void RL_Sim::JointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) void RL_Sim::RunModel() { - if(this->running_state == STATE_RL_RUNNING) + if(this->running_state == STATE_RL_RUNNING && simulation_running) { // this->obs.lin_vel = torch::tensor({{this->vel.linear.x, this->vel.linear.y, this->vel.linear.z}}); this->obs.ang_vel = torch::tensor(this->robot_state.imu.gyroscope).unsqueeze(0); @@ -199,7 +216,7 @@ void RL_Sim::RunModel() torch::Tensor origin_output_torques = this->ComputeTorques(this->obs.actions); - this->TorqueProtect(origin_output_torques); + // this->TorqueProtect(origin_output_torques); this->output_torques = torch::clamp(origin_output_torques, -(this->params.torque_limits), this->params.torque_limits); this->output_dof_pos = this->ComputePosition(this->obs.actions);