use_sim_time param load bug fix; hardware improve

src/a1_cpp/config/hardware_a1_mpc.yaml

@@ -31,10 +31,10 @@ a1_default_foot_pos_RR_x: -0.17
 a1_default_foot_pos_RR_y: -0.15
 a1_default_foot_pos_RR_z: -0.3
 
-a1_gait_counter_speed_FL: 1.1
-a1_gait_counter_speed_FR: 1.1
-a1_gait_counter_speed_RL: 1.1
-a1_gait_counter_speed_RR: 1.1
+a1_gait_counter_speed_FL: 1.4
+a1_gait_counter_speed_FR: 1.4
+a1_gait_counter_speed_RL: 1.4
+a1_gait_counter_speed_RR: 1.4
 
 # MPC parameters
 
@@ -56,21 +56,21 @@ q_weights_11: 0.3 # vel z
 
 q_weights_12: 0.0
 
-r_weights_0:  1e-5
-r_weights_1:  1e-5
-r_weights_2:  1e-6
+r_weights_0:  1e-2
+r_weights_1:  1e-2
+r_weights_2:  1e-3
 
-r_weights_3:  1e-5
-r_weights_4:  1e-5
-r_weights_5:  1e-6
+r_weights_3:  1e-2
+r_weights_4:  1e-2
+r_weights_5:  1e-3
 
-r_weights_6:  1e-5
-r_weights_7:  1e-5
-r_weights_8:  1e-6
+r_weights_6:  1e-2
+r_weights_7:  1e-2
+r_weights_8:  1e-3
 
-r_weights_9:  1e-6
-r_weights_10: 1e-6
-r_weights_11: 1e-6
+r_weights_9:  1e-2
+r_weights_10: 1e-2
+r_weights_11: 1e-3
 
 # foot swing controller parameters
 # force_tgt = km_foot * kp_foot * foot_pos_error
@@ -79,9 +79,9 @@ a1_kp_foot_x: 120.0
 a1_kp_foot_y: 120.0
 a1_kp_foot_z: 80.0
 
-a1_kd_foot_x: 5.0
-a1_kd_foot_y: 5.0
-a1_kd_foot_z: 8.0
+a1_kd_foot_x: 6.0
+a1_kd_foot_y: 6.0
+a1_kd_foot_z: 5.0
 
 a1_km_foot_x: 0.1
 a1_km_foot_y: 0.1

src/a1_cpp/src/A1RobotControl.cpp

@@ -462,7 +462,7 @@ Eigen::Matrix<double, 3, NUM_LEG> A1RobotControl::compute_grf(A1CtrlStates &stat
         double mpc_dt = 0.0025;
 
         // in simulation, use dt has no problem
-        if (use_sim_time == true) {
+        if (use_sim_time == "true") {
             mpc_dt = dt;
         }
 

src/a1_cpp/src/A1RobotControl.h

@@ -100,7 +100,7 @@ class A1RobotControl {
     //MPC does not start for the first 10 ticks to prevent uninitialized NAN goes into joint_torques
     int mpc_init_counter;
 
-    bool use_sim_time;
+    std::string use_sim_time;
 
     // filters
     MovingWindowFilter terrain_angle_filter;

src/a1_cpp/src/MainGazebo.cpp

@@ -28,15 +28,12 @@ int main(int argc, char **argv) {
     }
 
     // make sure the ROS infra using sim time, otherwise the controller cannot run with correct time steps
-    bool use_sim_time;
-    if (nh.getParam("/use_sim_time", use_sim_time)) {
-        if (use_sim_time != true) {
-            std::cout << "ROS must set use_sim_time to be true in order to use this program!" << std::endl;
+    std::string use_sim_time;
+    if (ros::param::get("/use_sim_time", use_sim_time)) {
+        if (use_sim_time != "true") {
+            std::cout << "ROS must set use_sim_time in order to use this program!" << std::endl;
             return -1;
         }
-    } else {
-        std::cout << "ROS must set use_sim_time in order to use this program!" << std::endl;
-        return -1;
     }
 
     // create a1 controller

src/a1_cpp/src/MainHardware.cpp

@@ -28,9 +28,9 @@ int main(int argc, char **argv) {
     }
 
     // make sure the ROS infra DO NOT use sim time, otherwise the controller cannot run with correct time steps
-    bool use_sim_time;
+    std::string use_sim_time;
     if (ros::param::get("/use_sim_time", use_sim_time)) {
-        if (use_sim_time != false) {
+        if (use_sim_time != "false") {
             std::cout << "hardware must have real time in order to use this program!" << std::endl;
             return -1;
         }

src/a1_cpp/src/MainIsaac.cpp

@@ -28,13 +28,14 @@ int main(int argc, char **argv) {
     }
 
     // make sure the ROS infra using sim time, otherwise the controller cannot run with correct time steps
-    bool use_sim_time;
-    if (nh.getParam("/use_sim_time", use_sim_time)) {
-        if (use_sim_time != true) {
-            std::cout << "ROS must set use_sim_time to be true in order to use this program!" << std::endl;
+    std::string use_sim_time;
+    if (ros::param::get("/use_sim_time", use_sim_time)) {
+        if (use_sim_time != "true") {
+            std::cout << "ROS must set use_sim_time in order to use this program!" << std::endl;
             return -1;
         }
-    } else {
+    }
+     else {
         std::cout << "ROS must set use_sim_time in order to use this program!" << std::endl;
         return -1;
     }