feat: change interaction logic

README.md

@@ -84,9 +84,11 @@ roslaunch rl_sar gazebo_<ROBOT>.launch
 
 Where \<ROBOT\> 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 **\<Enter\>** to toggle simulation start/stop.
+* **W** and **S** controls x-axis, **A** and **D** controls yaw, and **J** and **L** controls y-axis.
+* Press **\<Space\>** to sets all control commands to zero.
+* If robot falls down, press **R** to reset Gazebo environment.
 
 ### Physical Robots
 

README_CN.md

@@ -84,9 +84,12 @@ roslaunch rl_sar gazebo_<ROBOT>.launch
 
 其中 \<ROBOT\> 可以是 `a1` 或 `gr1t1`.
 
-按下键盘上的**0**键让机器人切换到默认站起姿态，按下**P**键切换到RL控制模式，任意状态按下**1**键切换到最初的趴下姿态。WS控制x，AD控制yaw，JL控制y。
+控制：
 
-按**R**重置Gazebo仿真环境。
+* 按 **\<Enter\>** 切换仿真器运行/停止。
+* **W** 和 **S** 控制x轴，**A** 和 **D** 控制yaw轴，**J** 和 **L** 控制y轴，按下空格重置控制指令。
+* 按 **\<Space\>** 将所有控制指令设置为零。
+* 如果机器人摔倒，按 **R** 重置Gazebo环境。
 
 ### 实物
 

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<std::string, ros::Publisher> joint_publishers;
     std::vector<robot_msgs::MotorCommand> joint_publishers_commands;
     void ModelStatesCallback(const gazebo_msgs::ModelStates::ConstPtr &msg);

src/rl_sar/launch/gazebo_a1.launch

@@ -32,7 +32,7 @@
     <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
     <!-- Set trunk and joint positions at startup -->
     <node pkg="gazebo_ros" type="spawn_model" name="urdf_spawner" respawn="false" output="screen"
-          args="-urdf -z 0.6 -model $(arg rname)_gazebo -param robot_description -unpause"/>
+          args="-urdf -z 0.6 -model $(arg rname)_gazebo -param robot_description"/>
 
     <!-- Load joint controller configurations from YAML file to parameter server -->
     <rosparam file="$(arg dollar)$(arg robot_path)/config/robot_control.yaml" command="load"/>

src/rl_sar/launch/gazebo_gr1t1.launch

@@ -28,7 +28,7 @@
     <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
     <!-- Set trunk and joint positions at startup -->
     <node pkg="gazebo_ros" type="spawn_model" name="urdf_spawner" respawn="false" output="screen"
-          args="-urdf -z 1.0 -model $(arg rname)_gazebo -param robot_description -unpause"/>
+          args="-urdf -z 1.0 -model $(arg rname)_gazebo -param robot_description"/>
 
     <!-- Load joint controller configurations from YAML file to parameter server -->
     <rosparam file="$(arg dollar)$(arg robot_path)/config/robot_control.yaml" command="load"/>

src/rl_sar/library/rl_sdk/rl_sdk.cpp

@@ -122,7 +122,7 @@ void RL::StateController(const RobotState<double> *state, RobotCommand<double> *
                 command->motor_command.kd[i] = this->params.fixed_kd[0][i].item<double>();
                 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<double> *state, RobotCommand<double> *
     // 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<double>();
@@ -205,7 +204,7 @@ void RL::StateController(const RobotState<double> *state, RobotCommand<double> *
                 command->motor_command.kd[i] = this->params.fixed_kd[0][i].item<double>();
                 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;
         }
     }

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);

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_msgs::SetModelState>("/gazebo/set_model_state");
-    
+    this->gazebo_pause_physics_client = nh.serviceClient<std_srvs::Empty>("/gazebo/pause_physics");
+    this->gazebo_unpause_physics_client = nh.serviceClient<std_srvs::Empty>("/gazebo/unpause_physics");
+
     // loop
     this->loop_keyboard = std::make_shared<LoopFunc>("loop_keyboard", 0.05, std::bind(&RL_Sim::KeyboardInterface, this));
     this->loop_control  = std::make_shared<LoopFunc>("loop_control", this->params.dt, std::bind(&RL_Sim::RobotControl, this));
@@ -131,8 +133,6 @@ void RL_Sim::SetCommand(const RobotCommand<double> *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);