able to work on wbc

README.md

@@ -14,6 +14,7 @@ Todo List:
 - [x] Contact Sensor
 - [ ] OCS2 Legged Control
 
+[![](http://i1.hdslb.com/bfs/archive/310e6208920985ac43015b2da31c01ec15e2c5f9.jpg)](https://www.bilibili.com/video/BV1aJbAeZEuo/)
 
 ## 1. Build
 * rosdep

controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp

@@ -74,7 +74,7 @@ namespace ocs2::legged_robot {
         current_observation_.input = optimized_input;
 
         wbc_timer_.startTimer();
-        vector_t x = wbc_->update(optimized_state, optimized_input, measuredRbdState_, planned_mode, period.seconds());
+        vector_t x = wbc_->update(optimized_state, optimized_input, measured_rbd_state_, planned_mode, period.seconds());
         wbc_timer_.endTimer();
 
         vector_t torque = x.tail(12);
@@ -86,6 +86,13 @@ namespace ocs2::legged_robot {
         // Safety check, if failed, stop the controller
         if (!safety_checker_->check(current_observation_, optimized_state, optimized_input)) {
             RCLCPP_ERROR(get_node()->get_logger(), "[Legged Controller] Safety check failed, stopping the controller.");
+            for (int i = 0; i < joint_names_.size(); i++) {
+                ctrl_comp_.joint_torque_command_interface_[i].get().set_value(0);
+                ctrl_comp_.joint_position_command_interface_[i].get().set_value(0);
+                ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(0);
+                ctrl_comp_.joint_kp_command_interface_[i].get().set_value(0.0);
+                ctrl_comp_.joint_kd_command_interface_[i].get().set_value(0.35);
+            }
             return controller_interface::return_type::ERROR;
         }
 
@@ -109,6 +116,7 @@ namespace ocs2::legged_robot {
         reference_file_ = auto_declare<std::string>("reference_file", reference_file_);
         gait_file_ = auto_declare<std::string>("gait_file", gait_file_);
 
+        // Load verbose parameter from the task file
         verbose_ = false;
         loadData::loadCppDataType(task_file_, "legged_robot_interface.verbose", verbose_);
 
@@ -129,9 +137,9 @@ namespace ocs2::legged_robot {
         setupMrt();
 
         // Visualization
-        CentroidalModelPinocchioMapping pinocchioMapping(legged_interface_->getCentroidalModelInfo());
+        CentroidalModelPinocchioMapping pinocchio_mapping(legged_interface_->getCentroidalModelInfo());
         eeKinematicsPtr_ = std::make_shared<PinocchioEndEffectorKinematics>(
-            legged_interface_->getPinocchioInterface(), pinocchioMapping,
+            legged_interface_->getPinocchioInterface(), pinocchio_mapping,
             legged_interface_->modelSettings().contactNames3DoF);
         // robotVisualizer_ = std::make_shared<LeggedRobotVisualizer>(leggedInterface_->getPinocchioInterface(),
         //                                                            leggedInterface_->getCentroidalModelInfo(), *eeKinematicsPtr_, nh);
@@ -182,6 +190,7 @@ namespace ocs2::legged_robot {
         // assign command interfaces
         for (auto &interface: command_interfaces_) {
             std::string interface_name = interface.get_interface_name();
+            std::cout << "interface_name: " << interface.get_prefix_name() << std::endl;
             if (const size_t pos = interface_name.find('/'); pos != std::string::npos) {
                 command_interface_map_[interface_name.substr(pos + 1)]->push_back(interface);
             } else {
@@ -314,10 +323,10 @@ namespace ocs2::legged_robot {
     }
 
     void Ocs2QuadrupedController::updateStateEstimation(const rclcpp::Time &time, const rclcpp::Duration &period) {
-        measuredRbdState_ = ctrl_comp_.estimator_->update(time, period);
+        measured_rbd_state_ = ctrl_comp_.estimator_->update(time, period);
         current_observation_.time += period.seconds();
         const scalar_t yaw_last = current_observation_.state(9);
-        current_observation_.state = rbd_conversions_->computeCentroidalStateFromRbdModel(measuredRbdState_);
+        current_observation_.state = rbd_conversions_->computeCentroidalStateFromRbdModel(measured_rbd_state_);
         current_observation_.state(9) = yaw_last + angles::shortest_angular_distance(
                                             yaw_last, current_observation_.state(9));
         current_observation_.mode = ctrl_comp_.estimator_->getMode();

controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h

@@ -127,7 +127,7 @@ namespace ocs2::legged_robot {
         std::shared_ptr<CentroidalModelRbdConversions> rbd_conversions_;
 
     private:
-        vector_t measuredRbdState_;
+        vector_t measured_rbd_state_;
         std::thread mpc_thread_;
         std::atomic_bool controller_running_{}, mpc_running_{};
         benchmark::RepeatedTimer mpc_timer_;

controllers/ocs2_quadruped_controller/src/interface/LeggedInterface.cpp

@@ -2,13 +2,24 @@
 // Created by qiayuan on 2022/7/16.
 //
 
-#include <memory>
+#include "ocs2_quadruped_controller/interface/LeggedInterface.h"
-#include <pinocchio/fwd.hpp>  // forward declarations must be included first.
 
+#include <memory>
+#include <ocs2_centroidal_model/AccessHelperFunctions.h>
+#include <ocs2_centroidal_model/CentroidalModelPinocchioMapping.h>
+#include <ocs2_centroidal_model/FactoryFunctions.h>
+#include <ocs2_centroidal_model/ModelHelperFunctions.h>
+#include <ocs2_core/misc/LoadData.h>
+#include <ocs2_core/misc/LoadStdVectorOfPair.h>
+#include <ocs2_core/soft_constraint/StateInputSoftConstraint.h>
+#include <ocs2_core/soft_constraint/StateSoftConstraint.h>
+#include <ocs2_legged_robot/dynamics/LeggedRobotDynamicsAD.h>
+#include <ocs2_oc/synchronized_module/SolverSynchronizedModule.h>
+#include <ocs2_pinocchio_interface/PinocchioEndEffectorKinematicsCppAd.h>
+#include <pinocchio/fwd.hpp>  // forward declarations must be included first.
 #include <pinocchio/algorithm/frames.hpp>
 #include <pinocchio/algorithm/jacobian.hpp>
 
-#include "ocs2_quadruped_controller/interface/LeggedInterface.h"
 #include "ocs2_quadruped_controller/interface/LeggedRobotPreComputation.h"
 #include "ocs2_quadruped_controller/interface/constraint/FrictionConeConstraint.h"
 #include "ocs2_quadruped_controller/interface/constraint/LeggedSelfCollisionConstraint.h"
@@ -18,25 +29,13 @@
 #include "ocs2_quadruped_controller/interface/cost/LeggedRobotQuadraticTrackingCost.h"
 #include "ocs2_quadruped_controller/interface/initialization/LeggedRobotInitializer.h"
 
-#include <ocs2_centroidal_model/AccessHelperFunctions.h>
-#include <ocs2_centroidal_model/CentroidalModelPinocchioMapping.h>
-#include <ocs2_centroidal_model/FactoryFunctions.h>
-#include <ocs2_centroidal_model/ModelHelperFunctions.h>
-#include <ocs2_core/misc/LoadData.h>
-#include <ocs2_core/misc/LoadStdVectorOfPair.h>
-#include <ocs2_core/soft_constraint/StateInputSoftConstraint.h>
-#include <ocs2_core/soft_constraint/StateSoftConstraint.h>
-#include <ocs2_oc/synchronized_module/SolverSynchronizedModule.h>
-#include <ocs2_pinocchio_interface/PinocchioEndEffectorKinematicsCppAd.h>
-
-#include <ocs2_legged_robot/dynamics/LeggedRobotDynamicsAD.h>
-
 // Boost
 #include <boost/filesystem/operations.hpp>
 #include <boost/filesystem/path.hpp>
 
 namespace ocs2::legged_robot {
-    LeggedInterface::LeggedInterface(const std::string &task_file, const std::string &urdf_file,
+    LeggedInterface::LeggedInterface(const std::string &task_file,
+                                     const std::string &urdf_file,
                                      const std::string &reference_file,
                                      const bool use_hard_friction_cone_constraint)
         : use_hard_friction_cone_constraint_(use_hard_friction_cone_constraint) {
@@ -85,7 +84,8 @@ namespace ocs2::legged_robot {
     }
 
 
-    void LeggedInterface::setupOptimalControlProblem(const std::string &task_file, const std::string &urdf_file,
+    void LeggedInterface::setupOptimalControlProblem(const std::string &task_file,
+                                                     const std::string &urdf_file,
                                                      const std::string &reference_file,
                                                      const bool verbose) {
         setupModel(task_file, urdf_file, reference_file);
@@ -147,9 +147,9 @@ namespace ocs2::legged_robot {
         rollout_ptr_ = std::make_unique<TimeTriggeredRollout>(*problem_ptr_->dynamicsPtr, rollout_settings_);
 
         // Initialization
-        constexpr bool extendNormalizedNomentum = true;
+        constexpr bool extend_normalized_momentum = true;
         initializer_ptr_ = std::make_unique<LeggedRobotInitializer>(centroidal_model_info_, *reference_manager_ptr_,
-                                                                    extendNormalizedNomentum);
+                                                                    extend_normalized_momentum);
     }
 
 
@@ -171,7 +171,7 @@ namespace ocs2::legged_robot {
 
     void LeggedInterface::setupReferenceManager(const std::string &taskFile, const std::string &urdfFile,
                                                 const std::string &referenceFile,
-                                                bool verbose) {
+                                                const bool verbose) {
         auto swingTrajectoryPlanner =
                 std::make_unique<SwingTrajectoryPlanner>(
                     loadSwingTrajectorySettings(taskFile, "swing_trajectory_config", verbose), 4);

controllers/ocs2_quadruped_controller/src/wbc/WbcBase.cpp

@@ -1,16 +1,18 @@
 //
 // Created by qiayuan on 2022/7/1.
 //
-#include <utility>
+
 #include "ocs2_quadruped_controller/wbc/WbcBase.h"
+
+#include <utility>
 #include <ocs2_centroidal_model/AccessHelperFunctions.h>
 #include <ocs2_centroidal_model/ModelHelperFunctions.h>
+#include <ocs2_core/misc/LoadData.h>
 #include <pinocchio/fwd.hpp>  // forward declarations must be included first.
 #include <pinocchio/algorithm/centroidal.hpp>
 #include <pinocchio/algorithm/crba.hpp>
 #include <pinocchio/algorithm/frames.hpp>
 #include <pinocchio/algorithm/rnea.hpp>
-#include <ocs2_core/misc/LoadData.h>
 
 namespace ocs2::legged_robot {
     WbcBase::WbcBase(const PinocchioInterface &pinocchioInterface, CentroidalModelInfo info,
@@ -31,7 +33,7 @@ namespace ocs2::legged_robot {
                              scalar_t /*period*/) {
         contact_flag_ = modeNumber2StanceLeg(mode);
         num_contacts_ = 0;
-        for (bool flag: contact_flag_) {
+        for (const bool flag: contact_flag_) {
             if (flag) {
                 num_contacts_++;
             }
@@ -99,7 +101,7 @@ namespace ocs2::legged_robot {
     }
 
     Task WbcBase::formulateFloatingBaseEomTask() {
-        auto &data = pinocchio_interface_measured_.getData();
+        const auto &data = pinocchio_interface_measured_.getData();
 
         matrix_t s(info_.actuatedDofNum, info_.generalizedCoordinatesNum);
         s.block(0, 0, info_.actuatedDofNum, 6).setZero();

descriptions/go1_description/config/robot_control.yaml

@@ -79,12 +79,12 @@ ocs2_quadruped_controller:
       - FL_hip_joint
       - FL_thigh_joint
       - FL_calf_joint
-      - RL_hip_joint
-      - RL_thigh_joint
-      - RL_calf_joint
       - FR_hip_joint
       - FR_thigh_joint
       - FR_calf_joint
+      - RL_hip_joint
+      - RL_thigh_joint
+      - RL_calf_joint
       - RR_hip_joint
       - RR_thigh_joint
       - RR_calf_joint
@@ -103,8 +103,8 @@ ocs2_quadruped_controller:
 
     feet_names:
       - FL_foot
-      - RL_foot
       - FR_foot
+      - RL_foot
       - RR_foot
 
 

descriptions/go2_description/config/robot_control.yaml

@@ -79,12 +79,12 @@ ocs2_quadruped_controller:
       - FL_hip_joint
       - FL_thigh_joint
       - FL_calf_joint
-      - RL_hip_joint
-      - RL_thigh_joint
-      - RL_calf_joint
       - FR_hip_joint
       - FR_thigh_joint
       - FR_calf_joint
+      - RL_hip_joint
+      - RL_thigh_joint
+      - RL_calf_joint
       - RR_hip_joint
       - RR_thigh_joint
       - RR_calf_joint
@@ -103,11 +103,10 @@ ocs2_quadruped_controller:
 
     feet_names:
       - FL_foot
-      - RL_foot
       - FR_foot
+      - RL_foot
       - RR_foot
 
-
     imu_name: "imu_sensor"
     base_name: "base"