ad qpoases colcon

.gitmodules

@@ -1,3 +0,0 @@
-[submodule "submodules/qpOASES"]
-	path = submodules/qpOASES
-	url = https://github.com/coin-or/qpOASES

controllers/ocs2_quadruped_controller/CMakeLists.txt

@@ -19,11 +19,11 @@ set(CONTROLLER_INCLUDE_DEPENDS
         control_input_msgs
         angles
         nav_msgs
+        qpoases_colcon
 )
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
-find_package(qpOASES REQUIRED)
 foreach (Dependency IN ITEMS ${CONTROLLER_INCLUDE_DEPENDS})
     find_package(${Dependency} REQUIRED)
 endforeach ()
@@ -64,10 +64,6 @@ target_include_directories(${PROJECT_NAME}
 ament_target_dependencies(${PROJECT_NAME}
         ${CONTROLLER_INCLUDE_DEPENDS}
 )
-target_link_libraries(${PROJECT_NAME}
-        ${qpOASES_LIBRARY_DIR}/libqpOASES.a
-)
-
 pluginlib_export_plugin_description_file(controller_interface ocs2_quadruped_controller.xml)
 
 install(

controllers/ocs2_quadruped_controller/README.md

@@ -12,15 +12,6 @@ and [ocs2_ros2](https://github.com/legubiao/ocs2_ros2).
   colcon build --packages-up-to ocs2_legged_robot_ros
   colcon build --packages-up-to ocs2_self_collision
   ```
-* qpOASES
-  ```bash
-  cd ~/ros2_ws/src/quadruped_ros2
-  git submodule update --init --recursive
-  ```
-  also add below line to the Cmakelist.txt of qpOASES
-  ```cmake
-  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC")
-  ```
 
 ### 2.2 Build OCS2 Quadruped Controller
 
@@ -30,8 +21,12 @@ colcon build --packages-up-to ocs2_quadruped_controller
 ```
 
 ## 3. Launch
-
+* Unitree Go1 Robot
-* Go2 Robot
+  ```bash
+  source ~/ros2_ws/install/setup.bash
+  ros2 launch go1_description ocs2_control.launch.py
+  ```
+* Unitree Go2 Robot
   ```bash
   source ~/ros2_ws/install/setup.bash
   ros2 launch go2_description ocs2_control.launch.py

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/WbcBase.h

@@ -33,7 +33,7 @@ namespace ocs2::legged_robot {
 
         void updateDesired(const vector_t &stateDesired, const vector_t &inputDesired);
 
-        size_t getNumDecisionVars() const { return numDecisionVars_; }
+        size_t getNumDecisionVars() const { return num_decision_vars_; }
 
         Task formulateFloatingBaseEomTask();
 
@@ -49,20 +49,20 @@ namespace ocs2::legged_robot {
 
         Task formulateContactForceTask(const vector_t &inputDesired) const;
 
-        size_t numDecisionVars_;
+        size_t num_decision_vars_;
-        PinocchioInterface pinocchioInterfaceMeasured_, pinocchioInterfaceDesired_;
+        PinocchioInterface pinocchio_interface_measured_, pinocchio_interface_desired_;
         CentroidalModelInfo info_;
 
-        std::unique_ptr<PinocchioEndEffectorKinematics> eeKinematics_;
+        std::unique_ptr<PinocchioEndEffectorKinematics> ee_kinematics_;
         CentroidalModelPinocchioMapping mapping_;
 
-        vector_t qMeasured_, vMeasured_, inputLast_;
+        vector_t q_measured_, v_measured_, input_last_;
         matrix_t j_, dj_;
-        contact_flag_t contactFlag_{};
+        contact_flag_t contact_flag_{};
-        size_t numContacts_{};
+        size_t num_contacts_{};
 
         // Task Parameters:
-        vector_t torqueLimits_;
+        vector_t torque_limits_;
-        scalar_t frictionCoeff_{}, swingKp_{}, swingKd_{};
+        scalar_t friction_coeff_{}, swing_kp_{}, swing_kd_{};
     };
 } // namespace legged

controllers/ocs2_quadruped_controller/package.xml

@@ -13,7 +13,7 @@
   <depend>controller_interface</depend>
   <depend>pluginlib</depend>
   <depend>control_input_msgs</depend>
-  <depend>qpOASES</depend>
+  <depend>qpoases_colcon</depend>
   <depend>ocs2_self_collision</depend>
   <depend>angles</depend>
 

controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp

@@ -86,6 +86,7 @@ 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.");
+            return controller_interface::return_type::ERROR;
         }
 
         for (int i = 0; i < joint_names_.size(); i++) {

controllers/ocs2_quadruped_controller/src/interface/LeggedInterface.cpp

@@ -70,7 +70,7 @@ namespace ocs2::legged_robot {
 
         // Todo : load settings from ros param
         model_settings_.jointNames = {
-            "FF_hip_joint", "FL_thigh_joint", "FL_calf_joint",
+            "FL_hip_joint", "FL_thigh_joint", "FL_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"
@@ -160,7 +160,7 @@ namespace ocs2::legged_robot {
                 std::make_unique<PinocchioInterface>(
                     centroidal_model::createPinocchioInterface(urdf_file, model_settings_.jointNames));
 
-        // CentroidalModelInfo
+        // CentroidModelInfo
         centroidal_model_info_ = centroidal_model::createCentroidalModelInfo(
             *pinocchio_interface_ptr_, centroidal_model::loadCentroidalType(task_file),
             centroidal_model::loadDefaultJointState(pinocchio_interface_ptr_->getModel().nq - 6, reference_file),

controllers/ocs2_quadruped_controller/src/wbc/WbcBase.cpp

@@ -15,25 +15,25 @@
 namespace ocs2::legged_robot {
     WbcBase::WbcBase(const PinocchioInterface &pinocchioInterface, CentroidalModelInfo info,
                      const PinocchioEndEffectorKinematics &eeKinematics)
-        : pinocchioInterfaceMeasured_(pinocchioInterface),
+        : pinocchio_interface_measured_(pinocchioInterface),
-          pinocchioInterfaceDesired_(pinocchioInterface),
+          pinocchio_interface_desired_(pinocchioInterface),
           info_(std::move(info)),
+          ee_kinematics_(eeKinematics.clone()),
           mapping_(info_),
-          inputLast_(vector_t::Zero(info_.inputDim)),
+          input_last_(vector_t::Zero(info_.inputDim)) {
-          eeKinematics_(eeKinematics.clone()) {
+        num_decision_vars_ = info_.generalizedCoordinatesNum + 3 * info_.numThreeDofContacts + info_.actuatedDofNum;
-        numDecisionVars_ = info_.generalizedCoordinatesNum + 3 * info_.numThreeDofContacts + info_.actuatedDofNum;
+        q_measured_ = vector_t(info_.generalizedCoordinatesNum);
-        qMeasured_ = vector_t(info_.generalizedCoordinatesNum);
+        v_measured_ = vector_t(info_.generalizedCoordinatesNum);
-        vMeasured_ = vector_t(info_.generalizedCoordinatesNum);
     }
 
     vector_t WbcBase::update(const vector_t &stateDesired, const vector_t &inputDesired,
                              const vector_t &rbdStateMeasured, size_t mode,
                              scalar_t /*period*/) {
-        contactFlag_ = modeNumber2StanceLeg(mode);
+        contact_flag_ = modeNumber2StanceLeg(mode);
-        numContacts_ = 0;
+        num_contacts_ = 0;
-        for (bool flag: contactFlag_) {
+        for (bool flag: contact_flag_) {
             if (flag) {
-                numContacts_++;
+                num_contacts_++;
             }
         }
 
@@ -44,25 +44,25 @@ namespace ocs2::legged_robot {
     }
 
     void WbcBase::updateMeasured(const vector_t &rbdStateMeasured) {
-        qMeasured_.head<3>() = rbdStateMeasured.segment<3>(3);
+        q_measured_.head<3>() = rbdStateMeasured.segment<3>(3);
-        qMeasured_.segment<3>(3) = rbdStateMeasured.head<3>();
+        q_measured_.segment<3>(3) = rbdStateMeasured.head<3>();
-        qMeasured_.tail(info_.actuatedDofNum) = rbdStateMeasured.segment(6, info_.actuatedDofNum);
+        q_measured_.tail(info_.actuatedDofNum) = rbdStateMeasured.segment(6, info_.actuatedDofNum);
-        vMeasured_.head<3>() = rbdStateMeasured.segment<3>(info_.generalizedCoordinatesNum + 3);
+        v_measured_.head<3>() = rbdStateMeasured.segment<3>(info_.generalizedCoordinatesNum + 3);
-        vMeasured_.segment<3>(3) = getEulerAnglesZyxDerivativesFromGlobalAngularVelocity<scalar_t>(
+        v_measured_.segment<3>(3) = getEulerAnglesZyxDerivativesFromGlobalAngularVelocity<scalar_t>(
-            qMeasured_.segment<3>(3), rbdStateMeasured.segment<3>(info_.generalizedCoordinatesNum));
+            q_measured_.segment<3>(3), rbdStateMeasured.segment<3>(info_.generalizedCoordinatesNum));
-        vMeasured_.tail(info_.actuatedDofNum) = rbdStateMeasured.segment(
+        v_measured_.tail(info_.actuatedDofNum) = rbdStateMeasured.segment(
             info_.generalizedCoordinatesNum + 6, info_.actuatedDofNum);
 
-        const auto &model = pinocchioInterfaceMeasured_.getModel();
+        const auto &model = pinocchio_interface_measured_.getModel();
-        auto &data = pinocchioInterfaceMeasured_.getData();
+        auto &data = pinocchio_interface_measured_.getData();
 
         // For floating base EoM task
-        forwardKinematics(model, data, qMeasured_, vMeasured_);
+        forwardKinematics(model, data, q_measured_, v_measured_);
         computeJointJacobians(model, data);
         updateFramePlacements(model, data);
-        crba(model, data, qMeasured_);
+        crba(model, data, q_measured_);
         data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();
-        nonLinearEffects(model, data, qMeasured_, vMeasured_);
+        nonLinearEffects(model, data, q_measured_, v_measured_);
         j_ = matrix_t(3 * info_.numThreeDofContacts, info_.generalizedCoordinatesNum);
         for (size_t i = 0; i < info_.numThreeDofContacts; ++i) {
             Eigen::Matrix<scalar_t, 6, Eigen::Dynamic> jac;
@@ -73,7 +73,7 @@ namespace ocs2::legged_robot {
         }
 
         // For not contact motion task
-        computeJointJacobiansTimeVariation(model, data, qMeasured_, vMeasured_);
+        computeJointJacobiansTimeVariation(model, data, q_measured_, v_measured_);
         dj_ = matrix_t(3 * info_.numThreeDofContacts, info_.generalizedCoordinatesNum);
         for (size_t i = 0; i < info_.numThreeDofContacts; ++i) {
             Eigen::Matrix<scalar_t, 6, Eigen::Dynamic> jac;
@@ -85,27 +85,27 @@ namespace ocs2::legged_robot {
     }
 
     void WbcBase::updateDesired(const vector_t &stateDesired, const vector_t &inputDesired) {
-        const auto &model = pinocchioInterfaceDesired_.getModel();
+        const auto &model = pinocchio_interface_desired_.getModel();
-        auto &data = pinocchioInterfaceDesired_.getData();
+        auto &data = pinocchio_interface_desired_.getData();
 
-        mapping_.setPinocchioInterface(pinocchioInterfaceDesired_);
+        mapping_.setPinocchioInterface(pinocchio_interface_desired_);
         const auto qDesired = mapping_.getPinocchioJointPosition(stateDesired);
         forwardKinematics(model, data, qDesired);
         computeJointJacobians(model, data, qDesired);
         updateFramePlacements(model, data);
-        updateCentroidalDynamics(pinocchioInterfaceDesired_, info_, qDesired);
+        updateCentroidalDynamics(pinocchio_interface_desired_, info_, qDesired);
         const vector_t vDesired = mapping_.getPinocchioJointVelocity(stateDesired, inputDesired);
         forwardKinematics(model, data, qDesired, vDesired);
     }
 
     Task WbcBase::formulateFloatingBaseEomTask() {
-        auto &data = pinocchioInterfaceMeasured_.getData();
+        auto &data = pinocchio_interface_measured_.getData();
 
         matrix_t s(info_.actuatedDofNum, info_.generalizedCoordinatesNum);
         s.block(0, 0, info_.actuatedDofNum, 6).setZero();
         s.block(0, 6, info_.actuatedDofNum, info_.actuatedDofNum).setIdentity();
 
-        matrix_t a = (matrix_t(info_.generalizedCoordinatesNum, numDecisionVars_) << data.M, -j_.transpose(), -s.
+        matrix_t a = (matrix_t(info_.generalizedCoordinatesNum, num_decision_vars_) << data.M, -j_.transpose(), -s.
                       transpose()).finished();
         vector_t b = -data.nle;
 
@@ -113,7 +113,7 @@ namespace ocs2::legged_robot {
     }
 
     Task WbcBase::formulateTorqueLimitsTask() {
-        matrix_t d(2 * info_.actuatedDofNum, numDecisionVars_);
+        matrix_t d(2 * info_.actuatedDofNum, num_decision_vars_);
         d.setZero();
         matrix_t i = matrix_t::Identity(info_.actuatedDofNum, info_.actuatedDofNum);
         d.block(0, info_.generalizedCoordinatesNum + 3 * info_.numThreeDofContacts, info_.actuatedDofNum,
@@ -123,23 +123,23 @@ namespace ocs2::legged_robot {
                 info_.actuatedDofNum) = -i;
         vector_t f(2 * info_.actuatedDofNum);
         for (size_t l = 0; l < 2 * info_.actuatedDofNum / 3; ++l) {
-            f.segment<3>(3 * l) = torqueLimits_;
+            f.segment<3>(3 * l) = torque_limits_;
         }
 
         return {matrix_t(), vector_t(), d, f};
     }
 
     Task WbcBase::formulateNoContactMotionTask() {
-        matrix_t a(3 * numContacts_, numDecisionVars_);
+        matrix_t a(3 * num_contacts_, num_decision_vars_);
         vector_t b(a.rows());
         a.setZero();
         b.setZero();
         size_t j = 0;
         for (size_t i = 0; i < info_.numThreeDofContacts; i++) {
-            if (contactFlag_[i]) {
+            if (contact_flag_[i]) {
                 a.block(3 * j, 0, 3, info_.generalizedCoordinatesNum) = j_.block(
                     3 * i, 0, 3, info_.generalizedCoordinatesNum);
-                b.segment(3 * j, 3) = -dj_.block(3 * i, 0, 3, info_.generalizedCoordinatesNum) * vMeasured_;
+                b.segment(3 * j, 3) = -dj_.block(3 * i, 0, 3, info_.generalizedCoordinatesNum) * v_measured_;
                 j++;
             }
         }
@@ -148,11 +148,11 @@ namespace ocs2::legged_robot {
     }
 
     Task WbcBase::formulateFrictionConeTask() {
-        matrix_t a(3 * (info_.numThreeDofContacts - numContacts_), numDecisionVars_);
+        matrix_t a(3 * (info_.numThreeDofContacts - num_contacts_), num_decision_vars_);
         a.setZero();
         size_t j = 0;
         for (size_t i = 0; i < info_.numThreeDofContacts; ++i) {
-            if (!contactFlag_[i]) {
+            if (!contact_flag_[i]) {
                 a.block(3 * j++, info_.generalizedCoordinatesNum + 3 * i, 3, 3) = matrix_t::Identity(3, 3);
             }
         }
@@ -161,16 +161,16 @@ namespace ocs2::legged_robot {
 
         matrix_t frictionPyramic(5, 3); // clang-format off
         frictionPyramic << 0, 0, -1,
-                1, 0, -frictionCoeff_,
+                1, 0, -friction_coeff_,
-                -1, 0, -frictionCoeff_,
+                -1, 0, -friction_coeff_,
-                0, 1, -frictionCoeff_,
+                0, 1, -friction_coeff_,
-                0, -1, -frictionCoeff_; // clang-format on
+                0, -1, -friction_coeff_; // clang-format on
 
-        matrix_t d(5 * numContacts_ + 3 * (info_.numThreeDofContacts - numContacts_), numDecisionVars_);
+        matrix_t d(5 * num_contacts_ + 3 * (info_.numThreeDofContacts - num_contacts_), num_decision_vars_);
         d.setZero();
         j = 0;
         for (size_t i = 0; i < info_.numThreeDofContacts; ++i) {
-            if (contactFlag_[i]) {
+            if (contact_flag_[i]) {
                 d.block(5 * j++, info_.generalizedCoordinatesNum + 3 * i, 5, 3) = frictionPyramic;
             }
         }
@@ -180,26 +180,26 @@ namespace ocs2::legged_robot {
     }
 
     Task WbcBase::formulateBaseAccelTask(const vector_t &stateDesired, const vector_t &inputDesired, scalar_t period) {
-        matrix_t a(6, numDecisionVars_);
+        matrix_t a(6, num_decision_vars_);
         a.setZero();
         a.block(0, 0, 6, 6) = matrix_t::Identity(6, 6);
 
-        vector_t jointAccel = centroidal_model::getJointVelocities(inputDesired - inputLast_, info_) / period;
+        vector_t jointAccel = centroidal_model::getJointVelocities(inputDesired - input_last_, info_) / period;
-        inputLast_ = inputDesired;
+        input_last_ = inputDesired;
-        mapping_.setPinocchioInterface(pinocchioInterfaceDesired_);
+        mapping_.setPinocchioInterface(pinocchio_interface_desired_);
 
-        const auto &model = pinocchioInterfaceDesired_.getModel();
+        const auto &model = pinocchio_interface_desired_.getModel();
-        auto &data = pinocchioInterfaceDesired_.getData();
+        auto &data = pinocchio_interface_desired_.getData();
         const auto qDesired = mapping_.getPinocchioJointPosition(stateDesired);
         const vector_t vDesired = mapping_.getPinocchioJointVelocity(stateDesired, inputDesired);
 
-        const auto &A = getCentroidalMomentumMatrix(pinocchioInterfaceDesired_);
+        const auto &A = getCentroidalMomentumMatrix(pinocchio_interface_desired_);
         const Matrix6 Ab = A.template leftCols<6>();
         const auto AbInv = computeFloatingBaseCentroidalMomentumMatrixInverse(Ab);
         const auto Aj = A.rightCols(info_.actuatedDofNum);
         const auto ADot = dccrba(model, data, qDesired, vDesired);
         Vector6 centroidalMomentumRate = info_.robotMass * getNormalizedCentroidalMomentumRate(
-                                             pinocchioInterfaceDesired_, info_, inputDesired);
+                                             pinocchio_interface_desired_, info_, inputDesired);
         centroidalMomentumRate.noalias() -= ADot * vDesired;
         centroidalMomentumRate.noalias() -= Aj * jointAccel;
 
@@ -209,25 +209,25 @@ namespace ocs2::legged_robot {
     }
 
     Task WbcBase::formulateSwingLegTask() {
-        eeKinematics_->setPinocchioInterface(pinocchioInterfaceMeasured_);
+        ee_kinematics_->setPinocchioInterface(pinocchio_interface_measured_);
-        std::vector<vector3_t> posMeasured = eeKinematics_->getPosition(vector_t());
+        std::vector<vector3_t> posMeasured = ee_kinematics_->getPosition(vector_t());
-        std::vector<vector3_t> velMeasured = eeKinematics_->getVelocity(vector_t(), vector_t());
+        std::vector<vector3_t> velMeasured = ee_kinematics_->getVelocity(vector_t(), vector_t());
-        eeKinematics_->setPinocchioInterface(pinocchioInterfaceDesired_);
+        ee_kinematics_->setPinocchioInterface(pinocchio_interface_desired_);
-        std::vector<vector3_t> posDesired = eeKinematics_->getPosition(vector_t());
+        std::vector<vector3_t> posDesired = ee_kinematics_->getPosition(vector_t());
-        std::vector<vector3_t> velDesired = eeKinematics_->getVelocity(vector_t(), vector_t());
+        std::vector<vector3_t> velDesired = ee_kinematics_->getVelocity(vector_t(), vector_t());
 
-        matrix_t a(3 * (info_.numThreeDofContacts - numContacts_), numDecisionVars_);
+        matrix_t a(3 * (info_.numThreeDofContacts - num_contacts_), num_decision_vars_);
         vector_t b(a.rows());
         a.setZero();
         b.setZero();
         size_t j = 0;
         for (size_t i = 0; i < info_.numThreeDofContacts; ++i) {
-            if (!contactFlag_[i]) {
+            if (!contact_flag_[i]) {
-                vector3_t accel = swingKp_ * (posDesired[i] - posMeasured[i]) + swingKd_ * (
+                vector3_t accel = swing_kp_ * (posDesired[i] - posMeasured[i]) + swing_kd_ * (
                                       velDesired[i] - velMeasured[i]);
                 a.block(3 * j, 0, 3, info_.generalizedCoordinatesNum) = j_.block(
                     3 * i, 0, 3, info_.generalizedCoordinatesNum);
-                b.segment(3 * j, 3) = accel - dj_.block(3 * i, 0, 3, info_.generalizedCoordinatesNum) * vMeasured_;
+                b.segment(3 * j, 3) = accel - dj_.block(3 * i, 0, 3, info_.generalizedCoordinatesNum) * v_measured_;
                 j++;
             }
         }
@@ -236,7 +236,7 @@ namespace ocs2::legged_robot {
     }
 
     Task WbcBase::formulateContactForceTask(const vector_t &inputDesired) const {
-        matrix_t a(3 * info_.numThreeDofContacts, numDecisionVars_);
+        matrix_t a(3 * info_.numThreeDofContacts, num_decision_vars_);
         vector_t b(a.rows());
         a.setZero();
 
@@ -248,24 +248,24 @@ namespace ocs2::legged_robot {
         return {a, b, matrix_t(), vector_t()};
     }
 
-    void WbcBase::loadTasksSetting(const std::string &taskFile, bool verbose) {
+    void WbcBase::loadTasksSetting(const std::string &taskFile, const bool verbose) {
         // Load task file
-        torqueLimits_ = vector_t(info_.actuatedDofNum / 4);
+        torque_limits_ = vector_t(info_.actuatedDofNum / 4);
-        loadData::loadEigenMatrix(taskFile, "torqueLimitsTask", torqueLimits_);
+        loadData::loadEigenMatrix(taskFile, "torqueLimitsTask", torque_limits_);
         if (verbose) {
             std::cerr << "\n #### Torque Limits Task:";
             std::cerr << "\n #### =============================================================================\n";
-            std::cerr << "\n #### HAA HFE KFE: " << torqueLimits_.transpose() << "\n";
+            std::cerr << "\n #### Hip_joint Thigh_joint Calf_joint: " << torque_limits_.transpose() << "\n";
             std::cerr << " #### =============================================================================\n";
         }
         boost::property_tree::ptree pt;
-        boost::property_tree::read_info(taskFile, pt);
+        read_info(taskFile, pt);
         std::string prefix = "frictionConeTask.";
         if (verbose) {
             std::cerr << "\n #### Friction Cone Task:";
             std::cerr << "\n #### =============================================================================\n";
         }
-        loadData::loadPtreeValue(pt, frictionCoeff_, prefix + "frictionCoefficient", verbose);
+        loadData::loadPtreeValue(pt, friction_coeff_, prefix + "frictionCoefficient", verbose);
         if (verbose) {
             std::cerr << " #### =============================================================================\n";
         }
@@ -274,7 +274,7 @@ namespace ocs2::legged_robot {
             std::cerr << "\n #### Swing Leg Task:";
             std::cerr << "\n #### =============================================================================\n";
         }
-        loadData::loadPtreeValue(pt, swingKp_, prefix + "kp", verbose);
+        loadData::loadPtreeValue(pt, swing_kp_, prefix + "kp", verbose);
-        loadData::loadPtreeValue(pt, swingKd_, prefix + "kd", verbose);
+        loadData::loadPtreeValue(pt, swing_kd_, prefix + "kd", verbose);
     }
 } // namespace legged

descriptions/go1_description/CMakeLists.txt

@@ -4,7 +4,7 @@ project(go1_description)
 find_package(ament_cmake REQUIRED)
 
 install(
-        DIRECTORY meshes xacro launch config
+        DIRECTORY meshes xacro launch config urdf
         DESTINATION share/${PROJECT_NAME}/
 )
 

descriptions/go1_description/README.md

@@ -14,8 +14,14 @@ source ~/ros2_ws/install/setup.bash
 ros2 launch go1_description visualize.launch.py
 ```
 
-## Launch Hardware Interface
+## Launch ROS2 Control
+* Unitree Guide Controller
   ```bash
   source ~/ros2_ws/install/setup.bash
-ros2 launch go1_description hardware.launch.py
+  ros2 launch go1_description unitree_guide.launch.py
+  ```
+* OCS2 Quadruped Controller
+  ```bash
+  source ~/ros2_ws/install/setup.bash
+  ros2 launch go1_description ocs2_control.launch.py
   ```

descriptions/go1_description/config/ocs2/gait.info

@@ -0,0 +1,255 @@
+list
+{
+  [0] stance
+  [1] trot
+  [2] standing_trot
+  [3] flying_trot
+  [4] pace
+  [5] standing_pace
+  [6] dynamic_walk
+  [7] static_walk
+  [8] amble
+  [9] lindyhop
+  [10] skipping
+  [11] pawup
+}
+
+stance
+ {
+   modeSequence
+   {
+     [0]     STANCE
+   }
+   switchingTimes
+   {
+     [0]     0.0
+     [1]     0.5
+   }
+}
+
+trot
+ {
+   modeSequence
+   {
+     [0]     LF_RH
+     [1]     RF_LH
+   }
+   switchingTimes
+   {
+     [0]     0.0
+     [1]     0.3
+     [2]     0.6
+   }
+}
+
+standing_trot
+{
+  modeSequence
+  {
+    [0]     LF_RH
+    [1]     STANCE
+    [2]     RF_LH
+    [3]     STANCE
+  }
+  switchingTimes
+  {
+    [0]     0.00
+    [1]     0.25
+    [2]     0.3
+    [3]     0.55
+    [4]     0.6
+  }
+}
+
+flying_trot
+{
+  modeSequence
+  {
+    [0]     LF_RH
+    [1]     FLY
+    [2]     RF_LH
+    [3]     FLY
+  }
+  switchingTimes
+  {
+    [0]     0.00
+    [1]     0.15
+    [2]     0.2
+    [3]     0.35
+    [4]     0.4
+  }
+}
+
+pace
+{
+  modeSequence
+  {
+    [0]     LF_LH
+    [1]     FLY
+    [2]     RF_RH
+    [3]     FLY
+  }
+  switchingTimes
+  {
+    [0]     0.0
+    [1]     0.28
+    [2]     0.30
+    [3]     0.58
+    [4]     0.60
+  }
+}
+
+standing_pace
+{
+  modeSequence
+  {
+    [0]     LF_LH
+    [1]     STANCE
+    [2]     RF_RH
+    [3]     STANCE
+  }
+  switchingTimes
+  {
+    [0]     0.0
+    [1]     0.30
+    [2]     0.35
+    [3]     0.65
+    [4]     0.70
+  }
+}
+
+dynamic_walk
+{
+  modeSequence
+  {
+    [0]     LF_RF_RH
+    [1]     RF_RH
+    [2]     RF_LH_RH
+    [3]     LF_RF_LH
+    [4]     LF_LH
+    [5]     LF_LH_RH
+  }
+  switchingTimes
+  {
+    [0]     0.0
+    [1]     0.2
+    [2]     0.3
+    [3]     0.5
+    [4]     0.7
+    [5]     0.8
+    [6]     1.0
+  }
+}
+
+static_walk
+{
+  modeSequence
+  {
+    [0]     LF_RF_RH
+    [1]     RF_LH_RH
+    [2]     LF_RF_LH
+    [3]     LF_LH_RH
+  }
+  switchingTimes
+  {
+    [0]     0.0
+    [1]     0.3
+    [2]     0.6
+    [3]     0.9
+    [4]     1.2
+  }
+}
+
+amble
+{
+  modeSequence
+  {
+    [0]     RF_LH
+    [1]     LF_LH
+    [2]     LF_RH
+    [3]     RF_RH
+  }
+  switchingTimes
+  {
+    [0]     0.0
+    [1]     0.15
+    [2]     0.40
+    [3]     0.55
+    [4]     0.80
+  }
+}
+
+lindyhop
+{
+  modeSequence
+  {
+    [0]     LF_RH
+    [1]     STANCE
+    [2]     RF_LH
+    [3]     STANCE
+    [4]     LF_LH
+    [5]     RF_RH
+    [6]     LF_LH
+    [7]     STANCE
+    [8]     RF_RH
+    [9]     LF_LH
+    [10]     RF_RH
+    [11]     STANCE
+  }
+  switchingTimes
+  {
+    [0]     0.00 ; Step 1
+    [1]     0.35 ; Stance
+    [2]     0.45 ; Step 2
+    [3]     0.80 ; Stance
+    [4]     0.90  ; Tripple step
+    [5]     1.125 ;
+    [6]     1.35  ;
+    [7]     1.70  ; Stance
+    [8]     1.80  ; Tripple step
+    [9]     2.025 ;
+    [10]    2.25  ;
+    [11]    2.60  ; Stance
+    [12]    2.70  ;
+  }
+}
+
+skipping
+{
+  modeSequence
+  {
+    [0]     LF_RH
+    [1]     FLY
+    [2]     LF_RH
+    [3]     FLY
+    [4]     RF_LH
+    [5]     FLY
+    [6]     RF_LH
+    [7]     FLY
+  }
+  switchingTimes
+  {
+    [0]     0.00
+    [1]     0.27
+    [2]     0.30
+    [3]     0.57
+    [4]     0.60
+    [5]     0.87
+    [6]     0.90
+    [7]     1.17
+    [8]     1.20
+  }
+}
+
+pawup
+{
+   modeSequence
+   {
+     [0]     RF_LH_RH
+   }
+   switchingTimes
+   {
+     [0]     0.0
+     [1]     2.0
+   }
+}

descriptions/go1_description/config/ocs2/reference.info

@@ -0,0 +1,46 @@
+targetDisplacementVelocity          0.5;
+targetRotationVelocity              1.57;
+
+comHeight                           0.3
+
+defaultJointState
+{
+   (0,0)  -0.20   ; FL_hip_joint
+   (1,0)   0.72   ; FL_thigh_joint
+   (2,0)  -1.44   ; FL_calf_joint
+   (3,0)  -0.20   ; RL_hip_joint
+   (4,0)   0.72   ; RL_thigh_joint
+   (5,0)  -1.44   ; RL_calf_joint
+   (6,0)   0.20   ; FR_hip_joint
+   (7,0)   0.72   ; FR_thigh_joint
+   (8,0)  -1.44   ; FR_calf_joint
+   (9,0)   0.20   ; RR_hip_joint
+   (10,0)  0.72   ; RR_thigh_joint
+   (11,0) -1.44   ; RR_calf_joint
+}
+
+initialModeSchedule
+{
+  modeSequence
+  {
+    [0]  STANCE
+    [1]  STANCE
+  }
+  eventTimes
+  {
+    [0]  0.5
+  }
+}
+
+defaultModeSequenceTemplate
+{
+  modeSequence
+  {
+    [0]  STANCE
+  }
+  switchingTimes
+  {
+    [0]  0.0
+    [1]  1.0
+  }
+}

descriptions/go1_description/config/ocs2/task.info

@@ -0,0 +1,319 @@
+centroidalModelType             0      // 0: FullCentroidalDynamics, 1: Single Rigid Body Dynamics
+
+legged_robot_interface
+{
+  verbose                               false  // show the loaded parameters
+}
+
+model_settings
+{
+  positionErrorGain             0.0
+  phaseTransitionStanceTime     0.1
+
+  verboseCppAd                  true
+  recompileLibrariesCppAd       false
+  modelFolderCppAd              /tmp/ocs2_quadruped_controller/go1
+}
+
+swing_trajectory_config
+{
+  liftOffVelocity               0.05
+  touchDownVelocity            -0.1
+  swingHeight                   0.08
+  swingTimeScale                0.15
+}
+
+; DDP settings
+ddp
+{
+  algorithm                       SLQ
+
+  nThreads                        3
+  threadPriority                  50
+
+  maxNumIterations                1
+  minRelCost                      1e-1
+  constraintTolerance             5e-3
+
+  displayInfo                     false
+  displayShortSummary             false
+  checkNumericalStability         false
+  debugPrintRollout               false
+  debugCaching                    false
+
+  AbsTolODE                       1e-5
+  RelTolODE                       1e-3
+  maxNumStepsPerSecond            10000
+  timeStep                        0.015
+  backwardPassIntegratorType      ODE45
+
+  constraintPenaltyInitialValue   20.0
+  constraintPenaltyIncreaseRate   2.0
+
+  preComputeRiccatiTerms          true
+
+  useFeedbackPolicy               false
+
+  strategy                        LINE_SEARCH
+  lineSearch
+  {
+    minStepLength                 1e-2
+    maxStepLength                 1.0
+    hessianCorrectionStrategy     DIAGONAL_SHIFT
+    hessianCorrectionMultiple     1e-5
+  }
+}
+
+
+; Multiple_Shooting SQP settings
+sqp
+{
+  nThreads                              3
+  dt                                    0.015
+  sqpIteration                          1
+  deltaTol                              1e-4
+  g_max                                 1e-2
+  g_min                                 1e-6
+  inequalityConstraintMu                0.1
+  inequalityConstraintDelta             5.0
+  projectStateInputEqualityConstraints  true
+  printSolverStatistics                 true
+  printSolverStatus                     false
+  printLinesearch                       false
+  useFeedbackPolicy                     false
+  integratorType                        RK2
+  threadPriority                        50
+}
+
+; Multiple_Shooting IPM settings
+ipm
+{
+  nThreads                              3
+  dt                                    0.015
+  ipmIteration                          1
+  deltaTol                              1e-4
+  g_max                                 10.0
+  g_min                                 1e-6
+  computeLagrangeMultipliers            true
+  printSolverStatistics                 true
+  printSolverStatus                     false
+  printLinesearch                       false
+  useFeedbackPolicy                     false
+  integratorType                        RK2
+  threadPriority                        50
+
+  initialBarrierParameter               1e-4
+  targetBarrierParameter                1e-4
+  barrierLinearDecreaseFactor           0.2
+  barrierSuperlinearDecreasePower       1.5
+  barrierReductionCostTol               1e-3
+  barrierReductionConstraintTol         1e-3
+
+  fractionToBoundaryMargin              0.995
+  usePrimalStepSizeForDual              false
+
+  initialSlackLowerBound                1e-4
+  initialDualLowerBound                 1e-4
+  initialSlackMarginRate                1e-2
+  initialDualMarginRate                 1e-2
+}
+
+; Rollout settings
+rollout
+{
+  AbsTolODE                       1e-5
+  RelTolODE                       1e-3
+  timeStep                        0.015
+  integratorType                  ODE45
+  maxNumStepsPerSecond            10000
+  checkNumericalStability         false
+}
+
+mpc
+{
+  timeHorizon                     1.0  ; [s]
+  solutionTimeWindow              -1   ; maximum [s]
+  coldStart                       false
+
+  debugPrint                      false
+
+  mpcDesiredFrequency             100  ; [Hz]
+  mrtDesiredFrequency             1000 ; [Hz] Useless
+}
+
+initialState
+{
+   ;; Normalized Centroidal Momentum: [linear, angular] ;;
+   (0,0)  0.0     ; vcom_x
+   (1,0)  0.0     ; vcom_y
+   (2,0)  0.0     ; vcom_z
+   (3,0)  0.0     ; L_x / robotMass
+   (4,0)  0.0     ; L_y / robotMass
+   (5,0)  0.0     ; L_z / robotMass
+
+   ;; Base Pose: [position, orientation] ;;
+   (6,0)  0.0     ; p_base_x
+   (7,0)  0.0     ; p_base_y
+   (8,0)  0.3     ; p_base_z
+   (9,0)  0.0     ; theta_base_z
+   (10,0) 0.0     ; theta_base_y
+   (11,0) 0.0     ; theta_base_x
+
+   ;; Leg Joint Positions: [FL, RL, FR, RR] ;;
+   (12,0) -0.20   ; FL_hip_joint
+   (13,0)  0.72   ; FL_thigh_joint
+   (14,0) -1.44   ; FL_calf_joint
+   (15,0) -0.20   ; RL_hip_joint
+   (16,0)  0.72   ; RL_thigh_joint
+   (17,0) -1.44   ; RL_calf_joint
+   (18,0)  0.20   ; FR_hip_joint
+   (19,0)  0.72   ; FR_thigh_joint
+   (20,0) -1.44   ; FR_calf_joint
+   (21,0)  0.20   ; RR_hip_joint
+   (22,0)  0.72   ; RR_thigh_joint
+   (23,0) -1.44   ; RR_calf_joint
+}
+
+; standard state weight matrix
+Q
+{
+  scaling 1e+0
+
+  ;; Normalized Centroidal Momentum: [linear, angular] ;;
+  (0,0)   15.0     ; vcom_x
+  (1,1)   15.0     ; vcom_y
+  (2,2)   100.0    ; vcom_z
+  (3,3)   10.0     ; L_x / robotMass
+  (4,4)   30.0     ; L_y / robotMass
+  (5,5)   30.0     ; L_z / robotMass
+
+  ;; Base Pose: [position, orientation] ;;
+  (6,6)   1000.0   ; p_base_x
+  (7,7)   1000.0   ; p_base_y
+  (8,8)   1500.0   ; p_base_z
+  (9,9)   100.0    ; theta_base_z
+  (10,10) 300.0    ; theta_base_y
+  (11,11) 300.0    ; theta_base_x
+
+  ;; Leg Joint Positions: [FL, RL, FR, RR] ;;
+  (12,12) 5.0     ; FL_hip_joint
+  (13,13) 5.0     ; FL_thigh_joint
+  (14,14) 2.5     ; FL_calf_joint
+  (15,15) 5.0     ; RL_hip_joint
+  (16,16) 5.0     ; RL_thigh_joint
+  (17,17) 2.5     ; RL_calf_joint
+  (18,18) 5.0     ; FR_hip_joint
+  (19,19) 5.0     ; FR_thigh_joint
+  (20,20) 2.5     ; FR_calf_joint
+  (21,21) 5.0     ; RR_hip_joint
+  (22,22) 5.0     ; RR_thigh_joint
+  (23,23) 2.5     ; RR_calf_joint
+}
+
+; control weight matrix
+R
+{
+  scaling 1e-3
+
+  ;; Feet Contact Forces: [FL, FR, RL, RR] ;;
+  (0,0)   1.0       ; front_left_force
+  (1,1)   1.0       ; front_left_force
+  (2,2)   1.0       ; front_left_force
+  (3,3)   1.0       ; front_right_force
+  (4,4)   1.0       ; front_right_force
+  (5,5)   1.0       ; front_right_force
+  (6,6)   1.0       ; rear_left_force
+  (7,7)   1.0       ; rear_left_force
+  (8,8)   1.0       ; rear_left_force
+  (9,9)   1.0       ; rear_right_force
+  (10,10) 1.0       ; rear_right_force
+  (11,11) 1.0       ; rear_right_force
+
+  ;; foot velocity relative to base: [FL, RL, FR, RR] (uses the Jacobian at nominal configuration) ;;
+  (12,12) 5000.0    ; x
+  (13,13) 5000.0    ; y
+  (14,14) 5000.0    ; z
+  (15,15) 5000.0    ; x
+  (16,16) 5000.0    ; y
+  (17,17) 5000.0    ; z
+  (18,18) 5000.0    ; x
+  (19,19) 5000.0    ; y
+  (20,20) 5000.0    ; z
+  (21,21) 5000.0    ; x
+  (22,22) 5000.0    ; y
+  (23,23) 5000.0    ; z
+}
+
+frictionConeSoftConstraint
+{
+  frictionCoefficient    0.3
+
+  ; relaxed log barrier parameters
+  mu                     0.1
+  delta                  5.0
+}
+
+selfCollision
+{
+  ; Self Collision raw object pairs
+  collisionObjectPairs
+  {
+  }
+
+  ; Self Collision pairs
+  collisionLinkPairs
+  {
+    [0] "FL_calf, FR_calf"
+    [1] "RL_calf, RR_calf"
+    [2] "FL_calf, RL_calf"
+    [3] "FR_calf, RR_calf"
+    [4] "FL_foot, FR_foot"
+    [5] "RL_foot, RR_foot"
+    [6] "FL_foot, RL_foot"
+    [7] "FR_foot, RR_foot"
+  }
+
+  minimumDistance  0.05
+
+  ; relaxed log barrier parameters
+  mu      1e-2
+  delta   1e-3
+}
+
+; Whole body control
+torqueLimitsTask
+{
+   (0,0)  33.5     ; HAA
+   (1,0)  33.5     ; HFE
+   (2,0)  33.5     ; KFE
+}
+
+frictionConeTask
+{
+  frictionCoefficient    0.3
+}
+
+swingLegTask
+{
+    kp                   350
+    kd                   37
+}
+
+weight
+{
+    swingLeg        100
+    baseAccel       1
+    contactForce    0.01
+}
+
+; State Estimation
+kalmanFilter
+{
+    footRadius                  0.02
+    imuProcessNoisePosition     0.02
+    imuProcessNoiseVelocity     0.02
+    footProcessNoisePosition    0.002
+    footSensorNoisePosition     0.005
+    footSensorNoiseVelocity     0.1
+    footHeightSensorNoise       0.01
+}

descriptions/go1_description/config/robot_control.yaml

@@ -74,19 +74,20 @@ unitree_guide_controller:
 ocs2_quadruped_controller:
   ros__parameters:
     update_rate: 500  # Hz
+
     joints:
-      - FR_hip_joint
-      - FR_thigh_joint
-      - FR_calf_joint
       - FL_hip_joint
       - FL_thigh_joint
       - FL_calf_joint
-      - RR_hip_joint
-      - RR_thigh_joint
-      - RR_calf_joint
       - RL_hip_joint
       - RL_thigh_joint
       - RL_calf_joint
+      - FR_hip_joint
+      - FR_thigh_joint
+      - FR_calf_joint
+      - RR_hip_joint
+      - RR_thigh_joint
+      - RR_calf_joint
 
     command_interfaces:
       - effort
@@ -101,10 +102,11 @@ ocs2_quadruped_controller:
       - velocity
 
     feet_names:
-      - FR_foot
       - FL_foot
-      - RR_foot
       - RL_foot
+      - FR_foot
+      - RR_foot
+
 
     imu_name: "imu_sensor"
     base_name: "base"
@@ -120,3 +122,10 @@ ocs2_quadruped_controller:
       - linear_acceleration.x
       - linear_acceleration.y
       - linear_acceleration.z
+
+    foot_force_name: "foot_force"
+    foot_force_interfaces:
+      - FL
+      - RL
+      - FR
+      - RR

descriptions/go1_description/launch/ocs2_control.launch.py

@@ -0,0 +1,122 @@
+import os
+
+import xacro
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler
+from launch.event_handlers import OnProcessExit
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from sympy.physics.vector.printing import params
+
+package_description = "go1_description"
+
+
+def process_xacro(context):
+    robot_type_value = context.launch_configurations['robot_type']
+    pkg_path = os.path.join(get_package_share_directory(package_description))
+    xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
+    robot_description_config = xacro.process_file(xacro_file, mappings={'robot_type': robot_type_value})
+    return (robot_description_config.toxml(), robot_type_value)
+
+
+def launch_setup(context, *args, **kwargs):
+    (robot_description, robot_type) = process_xacro(context)
+    robot_controllers = PathJoinSubstitution(
+        [
+            FindPackageShare(package_description),
+            "config",
+            "robot_control.yaml",
+        ]
+    )
+
+    robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
+        parameters=[
+            {
+                'publish_frequency': 20.0,
+                'use_tf_static': True,
+                'robot_description': robot_description,
+                'ignore_timestamp': True
+            }
+        ],
+    )
+
+    controller_manager = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[robot_controllers,
+                    {
+                        'urdf_file': os.path.join(get_package_share_directory(package_description), 'urdf', 'robot.urdf'),
+                        'task_file': os.path.join(get_package_share_directory(package_description), 'config', 'ocs2',
+                                                  'task.info'),
+                        'reference_file': os.path.join(get_package_share_directory(package_description), 'config',
+                                                       'ocs2', 'reference.info'),
+                        'gait_file': os.path.join(get_package_share_directory(package_description), 'config',
+                                                       'ocs2', 'gait.info')
+                    }],
+        output="both",
+    )
+
+    joint_state_publisher = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["joint_state_broadcaster",
+                   "--controller-manager", "/controller_manager"],
+    )
+
+    imu_sensor_broadcaster = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["imu_sensor_broadcaster",
+                   "--controller-manager", "/controller_manager"],
+    )
+
+    unitree_guide_controller = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["ocs2_quadruped_controller", "--controller-manager", "/controller_manager"]
+    )
+
+    return [
+        robot_state_publisher,
+        controller_manager,
+        joint_state_publisher,
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=joint_state_publisher,
+                on_exit=[imu_sensor_broadcaster],
+            )
+        ),
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=imu_sensor_broadcaster,
+                on_exit=[unitree_guide_controller],
+            )
+        ),
+    ]
+
+
+def generate_launch_description():
+    robot_type_arg = DeclareLaunchArgument(
+        'robot_type',
+        default_value='go2',
+        description='Type of the robot'
+    )
+
+    rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
+
+    return LaunchDescription([
+        robot_type_arg,
+        OpaqueFunction(function=launch_setup),
+        Node(
+            package='rviz2',
+            executable='rviz2',
+            name='rviz_ocs2',
+            output='screen',
+            arguments=["-d", rviz_config_file]
+        )
+    ])

descriptions/go2_description/config/robot_control.yaml

@@ -75,10 +75,6 @@ ocs2_quadruped_controller:
   ros__parameters:
     update_rate: 500  # Hz
 
-    #    urdf_file: "$(find legged_controllers)/config/$(arg robot_type)/robot.urdf"
-    #    task_file: "$(find go2_description)/config/ocs2/task.info"
-    #    reference_file: "$(find go2_description)/config/ocs2/reference.info"
-
     joints:
       - FL_hip_joint
       - FL_thigh_joint

libraries/qpoases_colcon/CMakeLists.txt

@@ -0,0 +1,60 @@
+cmake_minimum_required(VERSION 3.8)
+project(qpoases_colcon)
+
+set(CMAKE_BUILD_TYPE Release)
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+include(FetchContent)
+
+# Define directories
+set(QPOASES_DEVEL_PREFIX ${CMAKE_INSTALL_PREFIX} CACHE STRING "QPOASES install path")
+set(QPOASES_INCLUDE_DIR ${QPOASES_DEVEL_PREFIX}/include)
+set(QPOASES_LIB_DIR ${QPOASES_DEVEL_PREFIX}/lib)
+set(QPOASES_DOWNLOAD_DIR ${CMAKE_CURRENT_BINARY_DIR}/download)
+set(QPOASES_BUILD_DIR ${CMAKE_CURRENT_BINARY_DIR}/build)
+
+# Create directories if they do not exist
+file(MAKE_DIRECTORY ${QPOASES_INCLUDE_DIR})
+file(MAKE_DIRECTORY ${QPOASES_LIB_DIR})
+file(MAKE_DIRECTORY ${QPOASES_DOWNLOAD_DIR})
+file(MAKE_DIRECTORY ${QPOASES_BUILD_DIR})
+
+# QPOASES Settings
+set(BUILD_SHARED_LIBS ON CACHE STRING "Build shared libraries" FORCE)
+set(QPOASES_BUILD_EXAMPLES OFF CACHE BOOL "Examples disable")
+
+# Download & build source
+FetchContent_Declare(qpoasesDownload
+        GIT_REPOSITORY https://github.com/coin-or/qpOASES
+        UPDATE_COMMAND ""
+        SOURCE_DIR ${QPOASES_DOWNLOAD_DIR}
+        BINARY_DIR ${QPOASES_BUILD_DIR}
+        BUILD_COMMAND $(MAKE)
+        INSTALL_COMMAND "$(MAKE) install"
+)
+FetchContent_MakeAvailable(qpoasesDownload)
+
+# Copy header to where ament_cmake expects them
+file(COPY ${QPOASES_DOWNLOAD_DIR}/include/qpOASES.hpp DESTINATION ${QPOASES_INCLUDE_DIR})
+
+file(GLOB_RECURSE HEADERS "${QPOASES_DOWNLOAD_DIR}/include/qpOASES/*")
+foreach (HEADER_FILE ${HEADERS})
+  message(STATUS "FOUND HEADER: " ${HEADER_FILE})
+  file(COPY ${HEADER_FILE} DESTINATION ${QPOASES_INCLUDE_DIR}/qpOASES)
+endforeach ()
+
+file(GLOB_RECURSE HEADERS "${QPOASES_DOWNLOAD_DIR}/include/qpOASES/extras/*")
+foreach (HEADER_FILE ${HEADERS})
+  message(STATUS "FOUND HEADER: " ${HEADER_FILE})
+  file(COPY ${HEADER_FILE} DESTINATION ${QPOASES_INCLUDE_DIR}/qpOASES/extras)
+endforeach ()
+
+# Propagate dependencies
+ament_export_include_directories(${QPOASES_INCLUDE_DIR})
+ament_export_libraries(qpOASES)
+
+install(TARGETS qpOASES
+        LIBRARY DESTINATION lib)
+
+ament_package()

libraries/qpoases_colcon/LICENSE

@@ -0,0 +1,202 @@
+
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.

libraries/qpoases_colcon/package.xml

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>qpoases_colcon</name>
+  <version>0.0.0</version>
+  <description>Colcon wrapper for qpOASES</description>
+  <maintainer email="biao876990970@hotmail.com">biao</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

submodules/qpOASES

@@ -1 +0,0 @@
-Subproject commit 0b86dbf00c7fce34420bedc5914f71b176fe79d3