Fixed teleop_twist_joy, set IMU to 1000hz max, decrease simulator spe…

…ed to 1/5 for flat world
lunarlab-gatech · May 29, 2024 · 63b84a1 · 63b84a1
1 parent 9c0e297
commit 63b84a1
Show file tree

Hide file tree

Showing 6 changed files with 16 additions and 5 deletions.
diff --git a/external/teleop_twist_joy/config/pro_controller.config.yaml b/external/teleop_twist_joy/config/pro_controller.config.yaml
@@ -0,0 +1,9 @@
+axis_linear: 1  # Left thumb stick vertical
+scale_linear: 1.5
+scale_linear_turbo: 2.4
+
+axis_angular: 0  # Left thumb stick horizontal
+scale_angular: 1.5
+
+enable_button: 7  # ZR
+enable_turbo_button: 5  # R
diff --git a/external/teleop_twist_joy/launch/teleop.launch b/external/teleop_twist_joy/launch/teleop.launch
@@ -1,8 +1,9 @@
 <launch>
-  <arg name="joy_config" default="ps3" />
+  <arg name="joy_config" default="pro_controller" />
   <arg name="joy_dev" default="/dev/input/js0" />
   <arg name="config_filepath" default="$(find teleop_twist_joy)/config/$(arg joy_config).config.yaml" />
   <arg name="joy_topic" default="joy" />
+  <arg name="cmd_vel" default="robot_1/cmd_vel" />
 
   <node pkg="joy" type="joy_node" name="joy_node">
     <param name="dev" value="$(arg joy_dev)" />
@@ -14,5 +15,6 @@
   <node pkg="teleop_twist_joy" name="teleop_twist_joy" type="teleop_node" output="screen">
     <rosparam command="load" file="$(arg config_filepath)" />
     <remap from="joy" to="$(arg joy_topic)" />
+    <remap from="cmd_vel" to="$(arg cmd_vel)" />
   </node>
 </launch>
diff --git a/nmpc_controller/src/nmpc_controller.cpp b/nmpc_controller/src/nmpc_controller.cpp
@@ -205,7 +205,7 @@ NMPCController::NMPCController(ros::NodeHandle &nh, int robot_id) {
   app_ = IpoptApplicationFactory();
 
   app_->Options()->SetStringValue("print_timing_statistics", "no");
-  app_->Options()->SetStringValue("linear_solver", "ma27");
+  app_->Options()->SetStringValue("linear_solver", "ma57");
   app_->Options()->SetIntegerValue("print_level", 0);  // default=0, verbose=5
   app_->Options()->SetNumericValue("ma57_pre_alloc", 1.5);
   app_->Options()->SetStringValue("fixed_variable_treatment",

diff --git a/quad_simulator/gazebo_scripts/worlds/flat/flat.world b/quad_simulator/gazebo_scripts/worlds/flat/flat.world
@@ -2,8 +2,7 @@
   <world name='default'>
     <physics type="ode">
     <max_step_size>0.001</max_step_size>
-    <real_time_factor>1</real_time_factor>
-    <real_time_update_rate>1000</real_time_update_rate>
+    <real_time_update_rate>200</real_time_update_rate>
         <ode>
             <solver> 
               <iters>300</iters> 

diff --git a/quad_simulator/other/sensor_description/imu/model.sdf b/quad_simulator/other/sensor_description/imu/model.sdf
@@ -28,7 +28,7 @@
         <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
           <topicName>state/imu</topicName>
           <bodyName>link</bodyName>
-          <updateRateHZ>500.0</updateRateHZ>
+          <updateRateHZ>1000.0</updateRateHZ>
           <gaussianNoise>0.0</gaussianNoise>
           <xyzOffset>0 0 0</xyzOffset>
           <rpyOffset>0 0 0</rpyOffset>

diff --git a/quad_utils/launch/quad_gazebo.launch b/quad_utils/launch/quad_gazebo.launch
@@ -17,6 +17,7 @@
     <arg name="gui" value="$(arg gui)" />
     <arg name="physics" value="ode" />
     <arg name="verbose" value="$(arg gui)" />
+    <!--<arg name="extra_gazebo_args" default="lockstep"/> -->
   </include>
 
   <include file="$(find quad_utils)/launch/mapping.launch">