feat: fix bugs

.gitattributes

@@ -0,0 +1 @@
+*.sh eol=lf

README.md

@@ -131,7 +131,7 @@ Press the **R2** button on the controller to switch the robot to the default sta
 
 3. Use Ethernet cable to connect the computer and the motion control board
 
-    Due to the risk of damaging the interface and higher communication latency when using a Type-C connection, it is recommended to use an Ethernet cable for connection. Disconnect the cables between the main control and motion control board of the robot, and connect the computer and the motion control board directly with an Ethernet cable. It is recommended to remove the head and lead the cable out of the head opening. Be careful not to damage the cables during disassembly and assembly.
+    Due to the risk of damaging the interface and higher communication latency when using a Type-C connection, it is recommended to use an Ethernet cable for connection. Disconnect the cables between the main control and motion control board of the robot, and connect the computer and the motion control board directly with an Ethernet cable, then set the wired connection IPv4 of the computer to manual ` 192.168.55.100`. It is recommended to remove the head and lead the cable out of the head opening. Be careful not to damage the cables during disassembly and assembly.
 
     Initialize the robot's connection (this step needs to be done every time the robot is reconnected)
 

README_CN.md

@@ -131,7 +131,9 @@ rosrun rl_sar rl_real_a1
 
 3. 使用网线连接电脑和运动控制板
 
-    由于使用Type-C连接时调试碰撞易损坏接口，而且通信延迟较高，故推荐使用网线进行连接。需要将机器人拆开，断开断开主控和运动控制板的网线，将电脑和运动控制板使用网线直接连接。推荐拆掉头部并将网线从头部的开口引出。拆装时候注意不要损坏排线。
+    由于使用Type-C连接时调试碰撞易损坏接口，而且通信延迟较高，故推荐使用网线进行连接。需要将机器人拆开，断开断开主控和运动控制板的网线，将电脑和运动控制板使用网线直接连接，并设置电脑的有线连接IPv4为手动`192.168.55.100`。推荐拆掉头部并将网线从头部的开口引出。拆装时候注意不要损坏排线。
+
+    
 
     初始化机器人的连接(每次重新连接机器人都要执行此步骤)
 

src/rl_sar/include/rl_sim.hpp

@@ -59,6 +59,7 @@ private:
     std::vector<std::string> joint_names;
     std::vector<double> joint_positions;
     std::vector<double> joint_velocities;
+    std::vector<double> joint_efforts;
 
     int hip_scale_reduction_indices[4] = {0, 3, 6, 9};
 

src/rl_sar/src/rl_real_cyberdog.cpp

@@ -232,14 +232,14 @@ void RL_Real::run_keyboard()
                 case 'p': keyboard.robot_state = STATE_RL_INIT; break;
                 case '1': keyboard.robot_state = STATE_POS_GETDOWN; break;
                 case 'q': break;
-                case 'w': keyboard.x += 0.1; break;
-                case 's': keyboard.x -= 0.1; break;
-                case 'a': keyboard.yaw += 0.1; break;
-                case 'd': keyboard.yaw -= 0.1; break;
+                case 'w': keyboard.x += 0.5; break;
+                case 's': keyboard.x -= 0.5; break;
+                case 'a': keyboard.yaw += 0.5; break;
+                case 'd': keyboard.yaw -= 0.5; break;
                 case 'i': break;
                 case 'k': break;
-                case 'j': keyboard.y += 0.1; break;
-                case 'l': keyboard.y -= 0.1; break;
+                case 'j': keyboard.y += 0.5; break;
+                case 'l': keyboard.y -= 0.5; break;
                 case ' ': keyboard.x = 0; keyboard.y = 0; keyboard.yaw = 0; break;
                 default: break;
             }

src/rl_sar/src/rl_sim.cpp

@@ -3,6 +3,7 @@
 #define ROBOT_NAME "cyberdog1"
 
 // #define PLOT
+// #define CSV_LOGGER
 
 RL_Sim::RL_Sim()
 {
@@ -29,6 +30,7 @@ RL_Sim::RL_Sim()
 
     joint_positions = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
     joint_velocities = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+    joint_efforts = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
     plot_real_joint_pos.resize(12);
     plot_target_joint_pos.resize(12);
 
@@ -64,6 +66,10 @@ RL_Sim::RL_Sim()
     loop_plot    = std::make_shared<LoopFunc>("loop_plot"   , 0.002,    boost::bind(&RL_Sim::Plot,         this));
     loop_plot->start();
 #endif
+
+#ifdef CSV_LOGGER
+    CSVInit(ROBOT_NAME);
+#endif
 }
 
 RL_Sim::~RL_Sim()
@@ -111,6 +117,7 @@ void RL_Sim::JointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg)
 {
     joint_positions = msg->position;
     joint_velocities = msg->velocity;
+    joint_efforts = msg->effort;
 }
 
 void RL_Sim::RunModel()
@@ -156,6 +163,14 @@ void RL_Sim::RunModel()
 
     output_torques = this->ComputeTorques(actions);
     output_dof_pos = this->ComputePosition(actions);
+
+#ifdef CSV_LOGGER
+    torch::Tensor tau_est = torch::tensor({{joint_efforts[1], joint_efforts[2], joint_efforts[0],
+                                            joint_efforts[4], joint_efforts[5], joint_efforts[3],
+                                            joint_efforts[7], joint_efforts[8], joint_efforts[6],
+                                            joint_efforts[10], joint_efforts[11], joint_efforts[9]}});
+    CSVLogger(output_torques, tau_est, this->obs.dof_pos, output_dof_pos, this->obs.dof_vel);
+#endif
 }
 
 torch::Tensor RL_Sim::ComputeObservation()