From 63b84a114e2c838b0749786b44f2cfbd6012d8b9 Mon Sep 17 00:00:00 2001
From: DanielChaseButterfield <danielcbutter@gmail.com>
Date: Wed, 29 May 2024 10:07:05 -0400
Subject: [PATCH] Fixed teleop_twist_joy, set IMU to 1000hz max, decrease
 simulator speed to 1/5 for flat world

---
 .../teleop_twist_joy/config/pro_controller.config.yaml   | 9 +++++++++
 external/teleop_twist_joy/launch/teleop.launch           | 4 +++-
 nmpc_controller/src/nmpc_controller.cpp                  | 2 +-
 quad_simulator/gazebo_scripts/worlds/flat/flat.world     | 3 +--
 quad_simulator/other/sensor_description/imu/model.sdf    | 2 +-
 quad_utils/launch/quad_gazebo.launch                     | 1 +
 6 files changed, 16 insertions(+), 5 deletions(-)
 create mode 100644 external/teleop_twist_joy/config/pro_controller.config.yaml
 mode change 100644 => 100755 external/teleop_twist_joy/launch/teleop.launch

diff --git a/external/teleop_twist_joy/config/pro_controller.config.yaml b/external/teleop_twist_joy/config/pro_controller.config.yaml
new file mode 100644
index 00000000..9a7abe2a
--- /dev/null
+++ 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
old mode 100644
new mode 100755
index 9f95e6de..9c0ad810
--- 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
index 292a725f..d29155d6 100644
--- 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
index 9ad2bc08..7acad399 100644
--- 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
index 36aa2153..017f07bc 100644
--- 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
index 42fc5ebd..66dbae28 100644
--- 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">