V2.2.7

Author: Agnel-Wang

CMakeLists.txt

@@ -1,67 +1,23 @@
 cmake_minimum_required(VERSION 3.0)
 project(z1_controller LANGUAGES CXX)
-add_definitions(-std=c++14)
 set(PACKAGE_NAME z1_controller)
-set(CMAKE_CXX_FLAGS "$ENV{CXXFLAGS} -O3")
+set(CMAKE_CXX_FLAGS "$ENV{CXXFLAGS} -O3 -std=c++14 -pthread")
 
-# ----------------------options----------------------
-set(COMMUNICATION UDP)            # UDP
-# set(COMMUNICATION ROS)          # ROS
-
-
-# ----------------------configurations----------------------
-# COMMUNICATION
-if(${COMMUNICATION} STREQUAL "UDP")
-    set(SIMULATION OFF)
-elseif(${COMMUNICATION} STREQUAL "ROS")
-    add_definitions(-DCOMPILE_WITH_SIMULATION)
-    set(SIMULATION ON)
-else()
-    message(FATAL_ERROR "[CMake ERROR] The COMMUNICATION is error")
-endif()
-
-
-if(SIMULATION)
-    find_package(gazebo REQUIRED)
-    find_package(catkin REQUIRED COMPONENTS
-        controller_manager
-        genmsg
-        joint_state_controller
-        robot_state_publisher
-        roscpp
-        gazebo_ros
-        std_msgs
-        tf
-        geometry_msgs
-    )
-
-    catkin_package(
-        CATKIN_DEPENDS 
-    )
-
-    include_directories(
-        ${catkin_INCLUDE_DIRS}
-        ${GAZEBO_INCLUDE_DIRS}
-    )
-
-    link_directories(
-        ${GAZEBO_LIBRARY_DIRS}
-    )
-
-    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
-endif()
-
-set(EIGEN_PATH /usr/include/eigen3)
 find_package(Boost REQUIRED)
+find_package(Eigen3 REQUIRED)
 include_directories(
-    include
     ${Boost_INCLUDE_DIRS}
-    ${EIGEN_PATH}
+    ${Eigen3_INCLUDE_DIRS}
+    include
 )
 
 link_directories(lib)
-
 # ----------------------add executable----------------------
 set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR})
 add_executable(z1_ctrl main.cpp)
-target_link_libraries(z1_ctrl libZ1_${COMMUNICATION}_${CMAKE_HOST_SYSTEM_PROCESSOR}.so ${catkin_LIBRARIES} )
+target_link_libraries(z1_ctrl libZ1_${CMAKE_HOST_SYSTEM_PROCESSOR}.so)
+
+find_package(gazebo)
+if(${gazebo_FOUND})
+    add_subdirectory(sim)
+endif()

include/common/math/Filter.h

@@ -2,7 +2,7 @@
 #define FILTER
 
 #include <vector>
-#include <Eigen/Dense>
+#include <eigen3/Eigen/Dense>
 
 /*
     Low pass filter

include/common/math/mathTools.h

@@ -25,6 +25,15 @@ inline T min(const T val0, const Args... restVal){
     return val0 > (min<T>(restVal...)) ? val0 : min<T>(restVal...);
 }
 
+inline double clamp(const double& x, const double& low_value, const double& high_value) {
+    return (x > low_value) ? (x<high_value?x:high_value) : low_value;
+}
+
+inline double sign(const double& x, double threhold = 1e-6){
+    return std::fabs(x) < threhold ? 0 : ( x>0?1:-1 );
+}
+
+
 enum class TurnDirection{
     NOMATTER,
     POSITIVE,

include/common/utilities/typeTrans.h

@@ -3,7 +3,7 @@
 
 #include <iostream>
 #include <vector>
-#include <Eigen/Dense>
+#include <eigen3/Eigen/Dense>
 
 namespace typeTrans{
 

include/control/CtrlComponents.h

@@ -10,7 +10,6 @@
 #include "common/utilities/CSVTool.h"
 #include "model/ArmModel.h"
 #include "interface/IOUDP.h"
-#include "interface/IOROS.h"
 #include "control/armSDK.h"
 #include "model/unitree_gripper.h"
 
@@ -27,6 +26,7 @@ struct CtrlComponents{
     Z1Model *armModel;
     CSVTool *stateCSV;
     std::shared_ptr<Unitree_Gripper> gripper;
+    double gripper_max_tau = 10;
 
     SendCmd sendCmd; // cmd that receive from SDK 
     RecvState recvState;// state that send to SDK
@@ -41,14 +41,14 @@ struct CtrlComponents{
     bool isPlot;
     int trajChoose = 1;
     size_t armType = 36;
-
+    std::string ctrl_IP;
+    uint ctrl_port;
+    
     void geneObj();
     void writeData();
 private:
     void configProcess(int argc, char** argv);
 
-    std::string ctrl_IP;
-    uint ctrl_port;
     double _loadWeight;
 };