Separate State Estimate

tony-hj · Jul 23, 2022 · 17b9ffe · 17b9ffe
1 parent 2dc5092
commit 17b9ffe
Show file tree

Hide file tree

Showing 11 changed files with 128 additions and 7 deletions.
diff --git a/legged_controllers/CMakeLists.txt b/legged_controllers/CMakeLists.txt
@@ -13,6 +13,7 @@ find_package(catkin REQUIRED
         roscpp
         legged_common
         legged_interface
+        legged_estimation
         legged_wbc
         controller_interface
         ocs2_legged_robot_ros
@@ -31,6 +32,7 @@ catkin_package(
         roscpp
         legged_common
         legged_interface
+        legged_estimation
         legged_wbc
         controller_interface
         ocs2_legged_robot_ros
@@ -54,7 +56,6 @@ link_directories(
 ## Declare a cpp library
 add_library(${PROJECT_NAME}
         src/legged_controller.cpp
-        src/state_estimate.cpp
         )
 
 target_link_libraries(${PROJECT_NAME}

diff --git a/legged_controllers/config/default.yaml b/legged_controllers/config/default.yaml
@@ -4,3 +4,5 @@ controllers:
     publish_rate: 100
   legged_controller:
     type: legged/LeggedController
+  legged_cheater_controller:
+    type: legged/LeggedCheaterController
diff --git a/legged_controllers/include/legged_controllers/legged_controller.h b/legged_controllers/include/legged_controllers/legged_controller.h
@@ -3,9 +3,9 @@
 //
 
 #pragma once
-#include "legged_controllers/state_estimate.h"
 
 #include <legged_interface/legged_interface.h>
+#include <legged_estimation/state_estimate.h>
 #include <legged_wbc/wbc.h>
 
 #include <controller_interface/multi_interface_controller.h>
@@ -40,6 +40,10 @@ class LeggedController
 protected:
   virtual void setupLeggedInterface(const std::string& task_file, const std::string& urdf_file,
                                     const std::string& reference_file, bool verbose);
+  virtual void setupStateEstimate(LeggedInterface& legged_interface,
+                                  const std::vector<HybridJointHandle>& hybrid_joint_handles,
+                                  const std::vector<ContactSensorHandle>& contact_sensor_handles,
+                                  const hardware_interface::ImuSensorHandle& imu_sensor_handle);
 
   std::shared_ptr<LeggedInterface> legged_interface_;
   std::shared_ptr<Wbc> wbc_;
@@ -60,4 +64,13 @@ class LeggedController
   std::atomic_bool controller_running_, mpc_running_{};
 };
 
+class LeggedCheaterController : public LeggedController
+{
+protected:
+  virtual void setupStateEstimate(LeggedInterface& legged_interface,
+                                  const std::vector<HybridJointHandle>& hybrid_joint_handles,
+                                  const std::vector<ContactSensorHandle>& contact_sensor_handles,
+                                  const hardware_interface::ImuSensorHandle& imu_sensor_handle) override;
+};
+
 }  // namespace legged
diff --git a/legged_controllers/launch/load_controller.launch b/legged_controllers/launch/load_controller.launch
@@ -19,6 +19,7 @@
           args="load
           controllers/joint_state_controller
           controllers/legged_controller
+          controllers/legged_cheater_controller
 "/>
 
     <node pkg="ocs2_legged_robot_ros" type="legged_robot_target" name="legged_robot_command_target"

diff --git a/legged_controllers/legged_controllers_plugins.xml b/legged_controllers/legged_controllers_plugins.xml
@@ -3,7 +3,14 @@
     <class name="legged/LeggedController" type="legged::LeggedController"
            base_class_type="controller_interface::ControllerBase">
         <description>
-            The ocs2 controller.
+            The basic legged controller.
+        </description>
+    </class>
+
+    <class name="legged/LeggedCheaterController" type="legged::LeggedCheaterController"
+           base_class_type="controller_interface::ControllerBase">
+        <description>
+            The basic legged controller with cheat state estimator
         </description>
     </class>
 </library>
diff --git a/legged_controllers/package.xml b/legged_controllers/package.xml
@@ -13,6 +13,7 @@
     <depend>roscpp</depend>
     <depend>legged_common</depend>
     <depend>legged_interface</depend>
+    <depend>legged_estimation</depend>
     <depend>legged_wbc</depend>
 
     <depend>controller_interface</depend>

diff --git a/legged_controllers/src/legged_controller.cpp b/legged_controllers/src/legged_controller.cpp
@@ -85,6 +85,7 @@ bool LeggedController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHand
   });
   setThreadPriority(legged_interface_->sqpSettings().threadPriority, mpc_thread_);
 
+  // Hardware interface
   HybridJointInterface* hybrid_joint_interface = robot_hw->get<HybridJointInterface>();
   std::vector<std::string> joint_names{ "LF_HAA", "LF_HFE", "LF_KFE", "LH_HAA", "LH_HFE", "LH_KFE",
                                         "RF_HAA", "RF_HFE", "RF_KFE", "RH_HAA", "RH_HFE", "RH_KFE" };
@@ -95,10 +96,12 @@ bool LeggedController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHand
   std::vector<ContactSensorHandle> contact_handles;
   for (auto& name : legged_interface_->modelSettings().contactNames3DoF)
     contact_handles.push_back(contact_interface->getHandle(name));
-  state_estimate_ = std::make_shared<KalmanFilterEstimate>(
-      *legged_interface_, hybrid_joint_handles_, contact_handles,
-      robot_hw->get<hardware_interface::ImuSensorInterface>()->getHandle("unitree_imu"));
 
+  // State estimation
+  setupStateEstimate(*legged_interface_, hybrid_joint_handles_, contact_handles,
+                     robot_hw->get<hardware_interface::ImuSensorInterface>()->getHandle("unitree_imu"));
+
+  // Whole body control
   wbc_ = std::make_shared<Wbc>(*legged_interface_, ee_kinematics);
   return true;
 }
@@ -188,6 +191,26 @@ void LeggedController::setupLeggedInterface(const std::string& task_file, const
   legged_interface_->setupOptimalControlProblem(task_file, urdf_file, reference_file, verbose);
 }
 
+void LeggedController::setupStateEstimate(LeggedInterface& legged_interface,
+                                          const std::vector<HybridJointHandle>& hybrid_joint_handles,
+                                          const std::vector<ContactSensorHandle>& contact_sensor_handles,
+                                          const hardware_interface::ImuSensorHandle& imu_sensor_handle)
+{
+  state_estimate_ = std::make_shared<KalmanFilterEstimate>(*legged_interface_, hybrid_joint_handles_,
+                                                           contact_sensor_handles, imu_sensor_handle);
+}
+
+void LeggedCheaterController::setupStateEstimate(LeggedInterface& legged_interface,
+                                                 const std::vector<HybridJointHandle>& hybrid_joint_handles,
+                                                 const std::vector<ContactSensorHandle>& contact_sensor_handles,
+                                                 const hardware_interface::ImuSensorHandle& imu_sensor_handle)
+{
+  char error_message[] = "Cheater controller shouldn't be used with real hardware.";
+  ROS_ERROR_STREAM(error_message);
+  throw std::runtime_error(error_message);
+}
+
 }  // namespace legged
 
 PLUGINLIB_EXPORT_CLASS(legged::LeggedController, controller_interface::ControllerBase)
+PLUGINLIB_EXPORT_CLASS(legged::LeggedCheaterController, controller_interface::ControllerBase)
diff --git a/legged_estimation/CMakeLists.txt b/legged_estimation/CMakeLists.txt
@@ -0,0 +1,56 @@
+cmake_minimum_required(VERSION 3.10)
+project(legged_estimation)
+
+## Use C++14
+set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+add_definitions(-Wall)
+
+## Find catkin macros and libraries
+find_package(catkin REQUIRED
+        COMPONENTS
+        roscpp
+        legged_common
+        legged_interface
+        )
+
+find_package(PkgConfig REQUIRED)
+pkg_check_modules(pinocchio REQUIRED pinocchio)
+
+catkin_package(
+        INCLUDE_DIRS
+        include
+        LIBRARIES
+        ${PROJECT_NAME}
+        CATKIN_DEPENDS
+        roscpp
+        legged_common
+        legged_interface
+)
+
+###########
+## Build ##
+###########
+
+include_directories(
+        include
+        ${catkin_INCLUDE_DIRS}
+        ${pinocchio_INCLUDE_DIRS}
+)
+
+link_directories(
+        ${pinocchio_LIBRARY_DIRS}
+)
+
+## Declare a cpp library
+add_library(${PROJECT_NAME}
+        src/state_estimate.cpp
+        )
+
+target_link_libraries(${PROJECT_NAME}
+        ${catkin_LIBRARIES}
+        ${pinocchio_LIBRARIES}
+        )
+
+target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS})
diff --git a/...clude/legged_controllers/state_estimate.h → ...nclude/legged_estimation/state_estimate.h b/...clude/legged_controllers/state_estimate.h → ...nclude/legged_estimation/state_estimate.h
diff --git a/legged_estimation/package.xml b/legged_estimation/package.xml
@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<package format="2">
+    <name>legged_estimation</name>
+    <version>0.0.0</version>
+    <description>The legged_estimation packages.</description>
+    <maintainer email="[email protected]">Qiayuan Liao</maintainer>
+    <license>BSD</license>
+    <author email="[email protected]">Qiayuan Liao</author>
+
+    <!-- buildtool_depend: dependencies of the build process -->
+    <buildtool_depend>catkin</buildtool_depend>
+    <!-- depend: build, export, and execution dependency -->
+    <depend>roscpp</depend>
+    <depend>legged_common</depend>
+    <depend>legged_interface</depend>
+
+</package>
diff --git a/legged_controllers/src/state_estimate.cpp → legged_estimation/src/state_estimate.cpp b/legged_controllers/src/state_estimate.cpp → legged_estimation/src/state_estimate.cpp
@@ -4,7 +4,7 @@
 #include <pinocchio/algorithm/kinematics.hpp>
 #include <pinocchio/algorithm/frames.hpp>
 
-#include "legged_controllers/state_estimate.h"
+#include "legged_estimation/state_estimate.h"
 
 #include <ocs2_legged_robot/gait/MotionPhaseDefinition.h>
 #include <ocs2_robotic_tools/common/RotationTransforms.h>