update ver1

qm_common/CMakeLists.txt

@@ -0,0 +1,62 @@
+cmake_minimum_required(VERSION 3.10)
+project(qm_common)
+
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+find_package(catkin REQUIRED
+        COMPONENTS
+        roscpp
+        hardware_interface
+        )
+
+catkin_package(
+        INCLUDE_DIRS
+        include
+        LIBRARIES
+        CATKIN_DEPENDS
+        roscpp
+        hardware_interface
+        DEPENDS
+)
+
+include_directories(
+        include
+        ${catkin_INCLUDE_DIRS}
+)
+
+add_library(${PROJECT_NAME} INTERFACE)
+
+target_include_directories(${PROJECT_NAME} INTERFACE include)
+
+#########################
+###   CLANG TOOLING   ###
+#########################
+find_package(cmake_clang_tools QUIET)
+if (cmake_clang_tools_FOUND)
+    message(STATUS "Run clang tooling for target " ${PROJECT_NAME})
+    add_clang_tooling(
+            TARGETS ${PROJECT_NAME}
+            SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include
+            CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include
+            CF_WERROR
+    )
+endif (cmake_clang_tools_FOUND)
+
+#############
+## Install ##
+#############
+
+# Mark executables and/or libraries for installation
+install(
+        TARGETS ${PROJECT_NAME}
+        ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+        LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+        RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+# Mark cpp header files for installation
+install(
+        DIRECTORY include/${PROJECT_NAME}/
+        DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+        FILES_MATCHING PATTERN "*.h"
+)

qm_common/include/qm_common/debug_tool/dataIo.h

@@ -0,0 +1,26 @@
+//
+// Created by skywoodsz on 2023/3/19.
+//
+
+#ifndef SRC_DATAIO_H
+#define SRC_DATAIO_H
+#include <iostream>
+#include <Eigen/Dense>
+#include <fstream>
+#include <vector>
+
+using namespace std;
+using namespace Eigen;
+
+void saveData(string fileName, Eigen::MatrixXd matrix) {
+    //https://eigen.tuxfamily.org/dox/structEigen_1_1IOFormat.html
+    const static IOFormat CSVFormat(FullPrecision, DontAlignCols, ", ", "\n");
+
+    ofstream file(fileName);
+
+    file << matrix.format(CSVFormat);
+//    file.close();
+
+}
+
+#endif //SRC_DATAIO_H

qm_common/include/qm_common/hardware_interface/ContactSensorInterface.h

@@ -0,0 +1,36 @@
+//
+// Created by skywoodsz on 2023/2/19.
+//
+// ref: https://github.com/qiayuanliao/legged_control
+#pragma once
+#include <hardware_interface/internal/hardware_resource_manager.h>
+#include <hardware_interface/joint_state_interface.h>
+
+namespace qm {
+class ContactSensorHandle {
+ public:
+  ContactSensorHandle() = default;
+
+  ContactSensorHandle(const std::string& name, const bool* isContact) : name_(name), isContact_(isContact) {
+    if (isContact == nullptr) {
+      throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + name + "'. isContact pointer is null.");
+    }
+  }
+
+  std::string getName() const { return name_; }
+
+  bool isContact() const {
+    assert(isContact_);
+    return *isContact_;
+  }
+
+ private:
+  std::string name_;
+
+  const bool* isContact_ = {nullptr};
+};
+
+class ContactSensorInterface
+    : public hardware_interface::HardwareResourceManager<ContactSensorHandle, hardware_interface::DontClaimResources> {};
+
+}  // namespace qm

qm_common/include/qm_common/hardware_interface/HybridJointInterface.h

@@ -0,0 +1,93 @@
+//
+// Created by skywoodsz on 2023/2/19.
+//
+// ref: https://github.com/qiayuanliao/legged_control
+#pragma once
+#include <hardware_interface/internal/hardware_resource_manager.h>
+#include <hardware_interface/joint_state_interface.h>
+
+namespace qm {
+class HybridJointHandle : public hardware_interface::JointStateHandle {
+ public:
+  HybridJointHandle() = default;
+
+  HybridJointHandle(const JointStateHandle& js, double* posDes, double* velDes, double* kp, double* kd, double* ff)
+      : JointStateHandle(js), posDes_(posDes), velDes_(velDes), kp_(kp), kd_(kd), ff_(ff) {
+    if (posDes_ == nullptr) {
+      throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + js.getName() +
+                                                           "'. Position desired data pointer is null.");
+    }
+    if (velDes_ == nullptr) {
+      throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + js.getName() +
+                                                           "'. Velocity desired data pointer is null.");
+    }
+    if (kp_ == nullptr) {
+      throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + js.getName() + "'. Kp data pointer is null.");
+    }
+    if (kd_ == nullptr) {
+      throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + js.getName() + "'. Kd data pointer is null.");
+    }
+    if (ff_ == nullptr) {
+      throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + js.getName() +
+                                                           "'. Feedforward data pointer is null.");
+    }
+  }
+  void setPositionDesired(double cmd) {
+    assert(posDes_);
+    *posDes_ = cmd;
+  }
+  void setVelocityDesired(double cmd) {
+    assert(velDes_);
+    *velDes_ = cmd;
+  }
+  void setKp(double cmd) {
+    assert(kp_);
+    *kp_ = cmd;
+  }
+  void setKd(double cmd) {
+    assert(kd_);
+    *kd_ = cmd;
+  }
+  void setFeedforward(double cmd) {
+    assert(ff_);
+    *ff_ = cmd;
+  }
+  void setCommand(double pos_des, double vel_des, double kp, double kd, double ff) {
+    setPositionDesired(pos_des);
+    setVelocityDesired(vel_des);
+    setKp(kp);
+    setKd(kd);
+    setFeedforward(ff);
+  }
+  double getPositionDesired() {
+    assert(posDes_);
+    return *posDes_;
+  }
+  double getVelocityDesired() {
+    assert(velDes_);
+    return *velDes_;
+  }
+  double getKp() {
+    assert(kp_);
+    return *kp_;
+  }
+  double getKd() {
+    assert(kd_);
+    return *kd_;
+  }
+  double getFeedforward() {
+    assert(ff_);
+    return *ff_;
+  }
+
+ private:
+  double* posDes_ = {nullptr};
+  double* velDes_ = {nullptr};
+  double* kp_ = {nullptr};
+  double* kd_ = {nullptr};
+  double* ff_ = {nullptr};
+};
+
+class HybridJointInterface : public hardware_interface::HardwareResourceManager<HybridJointHandle, hardware_interface::ClaimResources> {};
+
+}  // namespace qm

qm_common/include/qm_common/math_tool/mathConvert.h

@@ -0,0 +1,87 @@
+//
+// Created by skywoodsz on 2023/2/19.
+//
+#ifndef SRC_MATHCONVERT_H
+#define SRC_MATHCONVERT_H
+
+#include <iostream>
+#include <numeric>
+#include <functional>
+#include <queue>
+#include <ocs2_core/Types.h>
+
+
+using namespace ocs2;
+
+class MovingAverage {
+public:
+    MovingAverage(int winSize, int dataSize){
+        pre_.resize(dataSize);
+        winSize_ = winSize;
+    };
+    vector_t filter(vector_t val, int winSize)
+    {
+        if(val.size() != pre_.size())
+        {
+            std::cerr<<"[MovingAverage]: data size is wrong!"<<std::endl;
+            return vector_t::Zero(pre_.size());
+        }
+
+        winSize_ = winSize;
+
+        dataQueue_.push(val);
+        if(dataQueue_.size() > winSize_)
+        {
+            vector_t tmp = dataQueue_.front();
+            pre_ -= tmp;
+            dataQueue_.pop();
+        }
+        pre_ += val;
+        return pre_ / dataQueue_.size();
+    };
+private:
+    int winSize_;
+    std::queue<vector_t> dataQueue_;
+    vector_t pre_;
+};
+
+
+
+template <typename T>
+T square(T a) {
+    return a * a;
+}
+
+/**
+ * convert quat to eular angle
+ * @param quat: w, x, y, z
+ * @return yaw, roll, pitch
+ */
+template <typename SCALAR_T>
+Eigen::Matrix<SCALAR_T, 3, 1> quatToZyx(const Eigen::Quaternion<SCALAR_T>& q) {
+    Eigen::Matrix<SCALAR_T, 3, 1> zyx;
+
+    SCALAR_T as = std::min(-2. * (q.x() * q.z() - q.w() * q.y()), .99999);
+    zyx(0) = std::atan2(2 * (q.x() * q.y() + q.w() * q.z()), square(q.w()) + square(q.x()) - square(q.y()) - square(q.z()));
+    zyx(1) = std::asin(as);
+    zyx(2) = std::atan2(2 * (q.y() * q.z() + q.w() * q.x()), square(q.w()) - square(q.x()) - square(q.y()) + square(q.z()));
+    return zyx;
+}
+
+//vector_t LimitJointAngle(vector_t joint_angle)
+//{
+//    vector_t joint_angle_limit(joint_angle.size());
+//    for (int i = 0; i < joint_angle.size(); ++i) {
+//         if(joint_angle[i] > M_PI || joint_angle[i] < -M_PI)
+//         {
+//             joint_angle_limit[i] = fmod(joint_angle[i], M_PI);
+//         }
+//         else
+//         {
+//             joint_angle_limit[i] = joint_angle[i];
+//         }
+//    }
+//    return joint_angle_limit;
+//}
+
+#endif //SRC_MATHCONVERT_H

qm_common/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<package format="2">
+    <name>qm_common</name>
+    <version>1.0.0</version>
+    <description>The qm_common package</description>
+
+    <maintainer email="[email protected]">skywoodsz</maintainer>
+
+    <license>BSD3</license>
+
+    <buildtool_depend>catkin</buildtool_depend>
+
+    <build_depend>cmake_clang_tools</build_depend>
+    <depend>roscpp</depend>
+    <depend>hardware_interface</depend>
+
+    <export>
+
+    </export>
+</package>

qm_control/CMakeLists.txt

@@ -0,0 +1,4 @@
+cmake_minimum_required(VERSION 3.10)
+project(qm_control)
+find_package(catkin REQUIRED)
+catkin_metapackage()

qm_control/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>qm_control</name>
+  <version>1.0.0</version>
+  <description>Metapackage for qm control</description>
+
+  <maintainer email="[email protected]">skywoodsz</maintainer>
+  <license>BSD3</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <exec_depend>qm_common</exec_depend>
+  <exec_depend>qm_controllers</exec_depend>
+  <exec_depend>qm_description</exec_depend>
+  <exec_depend>qm_estimation</exec_depend>
+  <exec_depend>qm_gazebo</exec_depend>
+  <exec_depend>qm_interface</exec_depend>
+  <exec_depend>qm_msgs</exec_depend>
+  <exec_depend>qm_wbc</exec_depend>
+
+   <export>
+   	  <metapackage />
+   </export>
+
+</package>