noetic_ekf_clean_real_ylo2

external/setup_deps.sh

@@ -16,7 +16,7 @@ mkdir coinbrew
 cd coinbrew
 wget https://raw.githubusercontent.com/coin-or/coinbrew/v1.0/coinbrew
 chmod u+x coinbrew
-./coinbrew fetch Ipopt --latest-release --no-prompt
+./coinbrew fetch Ipopt@3.13 --no-prompt
 cd ..
 if [ -d "./coinhsl" ] 
 then
@@ -26,7 +26,7 @@ else
     echo "Warning: HSL not found."
 fi
 cd coinbrew
-./coinbrew build Ipopt --latest-release --tests none --prefix=/usr/local --no-prompt --parallel-jobs=8
+./coinbrew build Ipopt --tests none --prefix=/usr/local --no-prompt --parallel-jobs=8
 cd ../..
 
 # Setup and build for rbdl

global_body_planner/CMakeLists.txt

@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.0.2)
 project(global_body_planner)
 
 ## Compile as C++14, supported in ROS Melodic and newer
-add_compile_options(-std=c++14 -ffast-math)
+add_compile_options(-std=c++17 -ffast-math)
 
 ## Set default cmake build type to release
 if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE)

global_body_planner/package.xml

@@ -68,6 +68,10 @@
   <exec_depend>grid_map_core</exec_depend>
   <exec_depend>grid_map_ros</exec_depend>
 
+  <depend>tf</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>grid_map</depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>

global_body_planner/setup_deps.sh

@@ -1,4 +1,4 @@
 sudo apt install -y ros-noetic-tf
 sudo apt install -y ros-noetic-tf2
 sudo apt install -y ros-noetic-tf2-geometry-msgs
-sudo apt install -y ros-noetic-grid-map
+sudo apt install -y ros-noetic-grid-map

global_body_planner/src/global_body_planner.cpp

@@ -307,7 +307,6 @@ bool GlobalBodyPlanner::callPlanner() {
               << " s" << std::endl;
     std::cout << std::endl;
   }
-  return true;
 }
 
 void GlobalBodyPlanner::waitForData() {

local_planner/CMakeLists.txt

@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.0.2)
 project(local_planner)
 
 ## Compile as C++14, supported in ROS Melodic and newer
-add_compile_options(-std=c++14)
+add_compile_options(-std=c++17)
 
 ## Set default cmake build type to release
 if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE)
@@ -28,11 +28,6 @@ find_package(Eigen3 REQUIRED)
 # find_package(OsqpEigen REQUIRED)
 find_package(PythonLibs 2.7)
 
-set(QPOASES_DIR "${CMAKE_CURRENT_SOURCE_DIR}/../external/qpOASES")
-set(QPOASES_INCLUDE_DIRS "${QPOASES_DIR}/include")
-set(QPOASES_LIBRARIES "${QPOASES_DIR}/build/libs")
-find_library(QPOASES NAMES qpOASES PATHS ${QPOASES_LIBRARIES} REQUIRED)
-
 catkin_package(
   INCLUDE_DIRS include
   CATKIN_DEPENDS roscpp std_msgs quad_msgs quad_utils grid_map_core grid_map_ros eigen_conversions nmpc_controller
@@ -50,7 +45,6 @@ include_directories(
   include
   ${catkin_INCLUDE_DIRS}
   ${EIGEN3_INCLUDE_DIRS}
-  ${QPOASES_INCLUDE_DIRS}
   ${PYTHON_INCLUDE_DIRS}
   /usr/local/include/coin-or
 )

local_planner/include/local_planner/local_planner.h

@@ -90,7 +90,7 @@ class LocalPlanner {
    */
   void publishLocalPlan();
 
-  /// Robot type: A1 or Spirit
+  /// Robot type: A1 or Ylo2
   std::string robot_name_;
 
   /// ROS subscriber for incoming terrain_map

local_planner/local_planner.yaml

@@ -1,8 +1,8 @@
 local_planner:
   update_rate: 333 # Hz
   timestep: 0.03 # s
-  horizon_length: 26 # timesteps
-  desired_height: 0.27 # m
+  horizon_length: 26 # timesteps # Try Increasing to 32
+  desired_height: 0.22 # m
   toe_radius: 0.02 # m
   cmd_vel_filter_const: 0.10
   cmd_vel_scale: 1.0

local_planner/setup_deps.sh

@@ -1 +1 @@
-sudo apt install -y ros-noetic-grid-map
+sudo apt install -y ros-noetic-grid-map

local_planner/src/local_footstep_planner.cpp

@@ -107,19 +107,17 @@ void LocalFootstepPlanner::computeContactSchedule(
   for (int i = 0; i < horizon_length_; i++) {
     // Leaping and landing
     if (ref_primitive_plan(i) == LEAP_STANCE) {
-      contact_schedule.at(i) = {true, true, true, true};
-    } else if (ref_primitive_plan(i) == FLIGHT) {
-      // Flight, check that min landing height is exceeded
-      double min_landing_height = 0.3;
-      double current_height =
-          body_plan(i, 2) - terrain_grid_.atPosition(
-                                "z_inpainted", body_plan.row(i).head<2>(),
-                                grid_map::InterpolationMethods::INTER_NEAREST);
-      if (current_height < min_landing_height && body_plan(i, 8) < 0) {
-        contact_schedule.at(i) = {true, true, true, true};
+      int leading_leg_liftoff_idx = std::min(i, horizon_length_ - 1);
+
+      if (ref_primitive_plan(leading_leg_liftoff_idx) == FLIGHT) {
+        contact_schedule.at(i) = {false, true, false, true};
       } else {
-        contact_schedule.at(i) = {false, false, false, false};
+        contact_schedule.at(i) = {true, true, true, true};
       }
+    } else if (ref_primitive_plan(i) == FLIGHT) {
+      // Flight
+      std::fill(contact_schedule.at(i).begin(), contact_schedule.at(i).end(),
+                false);
     } else if (ref_primitive_plan(i) == LAND_STANCE) {
       contact_schedule.at(i) = {true, true, true, true};
     }

local_planner/src/local_planner.cpp

@@ -13,7 +13,7 @@ LocalPlanner::LocalPlanner(ros::NodeHandle nh)
 
   // Load system parameters from launch file (not in config file)
   quad_utils::loadROSParamDefault(nh_, "robot_type", robot_name_,
-                                  std::string("spirit"));
+                                  std::string("ylo2"));
   quad_utils::loadROSParam(nh_, "/topics/terrain_map", terrain_map_topic);
   quad_utils::loadROSParam(nh_, "topics/global_plan", body_plan_topic);
   quad_utils::loadROSParam(nh_, "topics/state/ground_truth", robot_state_topic);
@@ -112,9 +112,7 @@ void LocalPlanner::initLocalBodyPlanner() {
   // Create nmpc wrapper class
   SystemID type;
   if (robot_name_ == "ylo2") {
-    type = A1; // TODO use my configs...
-  } else if (robot_name_ == "a1") {
-    type = A1;
+    type = YLO2;
   } else {
     ROS_WARN("WRONG ROBOT TYPE");
   }

nmpc_controller/CMakeLists.txt

@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.0.2)
 project(nmpc_controller)
 
 ## Compile as C++14, supported in ROS Melodic and newer
-add_compile_options(-std=c++14 -ffast-math)
+add_compile_options(-std=c++17 -ffast-math)
 
 ## Set default cmake build type to release
 if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE)
@@ -49,12 +49,15 @@ link_directories(/usr/local/lib)
 
 ## Declare a C++ library for control
 add_library(nmpc_controller
+
+  src/gen/eval_g_ylo2.cpp
+  src/gen/eval_jac_g_ylo2.cpp
+  src/gen/eval_hess_g_ylo2.cpp
+
   src/gen/eval_g_spirit.cpp
   src/gen/eval_jac_g_spirit.cpp
   src/gen/eval_hess_g_spirit.cpp
-  src/gen/eval_g_a1.cpp
-  src/gen/eval_jac_g_a1.cpp
-  src/gen/eval_hess_g_a1.cpp
+
   src/gen/eval_g_leg_simple.cpp
   src/gen/eval_jac_g_leg_simple.cpp
   src/gen/eval_hess_g_leg_simple.cpp

nmpc_controller/include/nmpc_controller/gen/eval_g_ylo2.h

@@ -0,0 +1,35 @@
+/* This file was automatically generated by CasADi 3.6.3.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int long long int
+#endif
+
+extern "C" int eval_g_ylo2(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem);
+extern "C" int eval_g_ylo2_alloc_mem(void);
+extern "C" int eval_g_ylo2_init_mem(int mem);
+extern "C" void eval_g_ylo2_free_mem(int mem);
+extern "C" int eval_g_ylo2_checkout(void);
+extern "C" void eval_g_ylo2_release(int mem);
+extern "C" void eval_g_ylo2_incref(void);
+extern "C" void eval_g_ylo2_decref(void);
+extern "C" casadi_int eval_g_ylo2_n_in(void);
+extern "C" casadi_int eval_g_ylo2_n_out(void);
+extern "C" casadi_real eval_g_ylo2_default_in(casadi_int i);
+extern "C" const char* eval_g_ylo2_name_in(casadi_int i);
+extern "C" const char* eval_g_ylo2_name_out(casadi_int i);
+extern "C" const casadi_int* eval_g_ylo2_sparsity_in(casadi_int i);
+extern "C" const casadi_int* eval_g_ylo2_sparsity_out(casadi_int i);
+extern "C" int eval_g_ylo2_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w);
+#define eval_g_ylo2_SZ_ARG 2
+#define eval_g_ylo2_SZ_RES 1
+#define eval_g_ylo2_SZ_IW 0
+#define eval_g_ylo2_SZ_W 52

nmpc_controller/include/nmpc_controller/gen/eval_hess_g_ylo2.h

@@ -0,0 +1,35 @@
+/* This file was automatically generated by CasADi 3.6.3.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int long long int
+#endif
+
+extern "C" int eval_hess_g_ylo2(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem);
+extern "C" int eval_hess_g_ylo2_alloc_mem(void);
+extern "C" int eval_hess_g_ylo2_init_mem(int mem);
+extern "C" void eval_hess_g_ylo2_free_mem(int mem);
+extern "C" int eval_hess_g_ylo2_checkout(void);
+extern "C" void eval_hess_g_ylo2_release(int mem);
+extern "C" void eval_hess_g_ylo2_incref(void);
+extern "C" void eval_hess_g_ylo2_decref(void);
+extern "C" casadi_int eval_hess_g_ylo2_n_in(void);
+extern "C" casadi_int eval_hess_g_ylo2_n_out(void);
+extern "C" casadi_real eval_hess_g_ylo2_default_in(casadi_int i);
+extern "C" const char* eval_hess_g_ylo2_name_in(casadi_int i);
+extern "C" const char* eval_hess_g_ylo2_name_out(casadi_int i);
+extern "C" const casadi_int* eval_hess_g_ylo2_sparsity_in(casadi_int i);
+extern "C" const casadi_int* eval_hess_g_ylo2_sparsity_out(casadi_int i);
+extern "C" int eval_hess_g_ylo2_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w);
+#define eval_hess_g_ylo2_SZ_ARG 3
+#define eval_hess_g_ylo2_SZ_RES 1
+#define eval_hess_g_ylo2_SZ_IW 0
+#define eval_hess_g_ylo2_SZ_W 138

nmpc_controller/include/nmpc_controller/gen/eval_jac_g_ylo2.h

@@ -0,0 +1,35 @@
+/* This file was automatically generated by CasADi 3.6.3.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int long long int
+#endif
+
+extern "C" int eval_jac_g_ylo2(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem);
+extern "C" int eval_jac_g_ylo2_alloc_mem(void);
+extern "C" int eval_jac_g_ylo2_init_mem(int mem);
+extern "C" void eval_jac_g_ylo2_free_mem(int mem);
+extern "C" int eval_jac_g_ylo2_checkout(void);
+extern "C" void eval_jac_g_ylo2_release(int mem);
+extern "C" void eval_jac_g_ylo2_incref(void);
+extern "C" void eval_jac_g_ylo2_decref(void);
+extern "C" casadi_int eval_jac_g_ylo2_n_in(void);
+extern "C" casadi_int eval_jac_g_ylo2_n_out(void);
+extern "C" casadi_real eval_jac_g_ylo2_default_in(casadi_int i);
+extern "C" const char* eval_jac_g_ylo2_name_in(casadi_int i);
+extern "C" const char* eval_jac_g_ylo2_name_out(casadi_int i);
+extern "C" const casadi_int* eval_jac_g_ylo2_sparsity_in(casadi_int i);
+extern "C" const casadi_int* eval_jac_g_ylo2_sparsity_out(casadi_int i);
+extern "C" int eval_jac_g_ylo2_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w);
+#define eval_jac_g_ylo2_SZ_ARG 2
+#define eval_jac_g_ylo2_SZ_RES 1
+#define eval_jac_g_ylo2_SZ_IW 0
+#define eval_jac_g_ylo2_SZ_W 122

nmpc_controller/include/nmpc_controller/nmpc_controller.h

@@ -103,7 +103,7 @@ class NMPCController {
 
   int robot_id_;
 
-  /// Robot type: A1 or Spirit
+  /// Robot type: Ylo2 or Ylo2
   std::string robot_ns_;
 
   /// Update rate for sending and receiving data;