Add crc calculation for python LowCmd dds package (can be used in both sdk and ros)

.gitignore

@@ -0,0 +1,4 @@
+build
+__pycache__
+*cpython*
+.vscode

CMakeLists.txt

@@ -3,7 +3,9 @@ cmake_minimum_required(VERSION 3.5)
 
 SET(CMAKE_CXX_STANDARD 17)
 
-include_directories(/usr/local/include/ddscxx /usr/local/include/iceoryx/v2.0.2)
+include_directories(include thirdparty/include thirdparty/include/ddscxx thirdparty/include/iceoryx/v2.0.2)
+link_directories(lib/${CMAKE_HOST_SYSTEM_PROCESSOR} thirdparty/lib/${CMAKE_HOST_SYSTEM_PROCESSOR})
+
 link_libraries(unitree_sdk2 ddsc ddscxx rt pthread)
 
 add_executable(test_publisher example/helloworld/publisher.cpp example/helloworld/HelloWorldData.cpp)
@@ -12,6 +14,7 @@ add_executable(high_follow_sin example/high_level/follow_sin.cpp)
 add_executable(sportmode_test example/high_level/sportmode_test.cpp)
 add_executable(low_level example/low_level/low_level.cpp)
 add_executable(stand_example_go2 example/low_level/stand_example_go2.cpp)
+add_executable(disable_sports_mode_go2 example/low_level/disable_sports_mode_go2.cpp)
 add_executable(stand_example_b2  example/low_level/stand_example_b2.cpp)
 add_executable(wireless example/wireless/wireless.cpp)
 add_executable(advanced_gamepad example/advanced_gamepad/main.cpp)

README.md

@@ -1,5 +1,12 @@
 # unitree_sdk2
-Unitree robot sdk version 2.
+Unitree robot sdk version 2 modified by @yihuai-gao
+
+## Additional features
+- python/crc_module.cpython-310-aarch64-linux-gnu.so to calculate crc for each message pack when running python ros2
+- build/disable_sports_mode_go2 to disable sports mode before running self-built controller
+
+usage: [README](python/README.md)
+
 
 ### Prebuild environment
 * OS  (Ubuntu 20.04 LTS)  

example/low_level/disable_sports_mode_go2.cpp

@@ -0,0 +1,308 @@
+#include <iostream>
+#include <stdio.h>
+#include <stdint.h>
+#include <math.h>
+#include <unitree/robot/channel/channel_publisher.hpp>
+#include <unitree/robot/channel/channel_subscriber.hpp>
+#include <unitree/idl/go2/LowState_.hpp>
+#include <unitree/idl/go2/LowCmd_.hpp>
+#include <unitree/common/time/time_tool.hpp>
+#include <unitree/common/thread/thread.hpp>
+#include <unitree/robot/go2/robot_state/robot_state_client.hpp>
+
+using namespace unitree::common;
+using namespace unitree::robot;
+using namespace unitree::robot::go2;
+
+#define TOPIC_LOWCMD "rt/lowcmd"
+#define TOPIC_LOWSTATE "rt/lowstate"
+
+constexpr double PosStopF = (2.146E+9f);
+constexpr double VelStopF = (16000.0f);
+
+class Custom
+{
+public:
+    explicit Custom(){}
+    ~Custom(){}
+
+    void Init();
+    void InitRobotStateClient();
+    int queryServiceStatus(const std::string& serviceName);
+    void activateService(const std::string& serviceName,int activate);
+private:
+    void InitLowCmd();
+    void LowStateMessageHandler(const void* messages);
+    void LowCmdWrite();
+
+private:
+    float Kp = 60.0;
+    float Kd = 5.0;
+    double time_consume = 0;
+    int rate_count = 0;
+    int sin_count = 0;
+    int motiontime = 0;
+    float dt = 0.002; // 0.001~0.01
+
+    RobotStateClient rsc;
+
+    unitree_go::msg::dds_::LowCmd_ low_cmd{};      // default init
+    unitree_go::msg::dds_::LowState_ low_state{};  // default init
+
+    /*publisher*/
+    ChannelPublisherPtr<unitree_go::msg::dds_::LowCmd_> lowcmd_publisher;
+    /*subscriber*/
+    ChannelSubscriberPtr<unitree_go::msg::dds_::LowState_> lowstate_subscriber;
+
+    /*LowCmd write thread*/
+    ThreadPtr lowCmdWriteThreadPtr;
+
+    float _targetPos_1[12] = {0.0, 1.36, -2.65, 0.0, 1.36, -2.65,
+                              -0.2, 1.36, -2.65, 0.2, 1.36, -2.65};
+
+    float _targetPos_2[12] = {0.0, 0.67, -1.3, 0.0, 0.67, -1.3,
+                              0.0, 0.67, -1.3, 0.0, 0.67, -1.3};
+
+    float _targetPos_3[12] = {-0.35, 1.36, -2.65, 0.35, 1.36, -2.65,
+                              -0.5, 1.36, -2.65, 0.5, 1.36, -2.65};
+
+    float _startPos[12];
+    float _duration_1 = 500;   
+    float _duration_2 = 500; 
+    float _duration_3 = 1000;   
+    float _duration_4 = 900;   
+    float _percent_1 = 0;    
+    float _percent_2 = 0;    
+    float _percent_3 = 0;    
+    float _percent_4 = 0;    
+
+    bool firstRun = true;
+    bool done = false;
+};
+
+uint32_t crc32_core(uint32_t* ptr, uint32_t len)
+{
+    unsigned int xbit = 0;
+    unsigned int data = 0;
+    unsigned int CRC32 = 0xFFFFFFFF;
+    const unsigned int dwPolynomial = 0x04c11db7;
+
+    for (unsigned int i = 0; i < len; i++)
+    {
+        xbit = 1 << 31;
+        data = ptr[i];
+        for (unsigned int bits = 0; bits < 32; bits++)
+        {
+            if (CRC32 & 0x80000000)
+            {
+                CRC32 <<= 1;
+                CRC32 ^= dwPolynomial;
+            }
+            else
+            {
+                CRC32 <<= 1;
+            }
+
+            if (data & xbit)
+                CRC32 ^= dwPolynomial;
+            xbit >>= 1;
+        }
+    }
+
+    return CRC32;
+}
+
+void Custom::Init()
+{
+    InitLowCmd();
+
+    /*create publisher*/
+    lowcmd_publisher.reset(new ChannelPublisher<unitree_go::msg::dds_::LowCmd_>(TOPIC_LOWCMD));
+    lowcmd_publisher->InitChannel();
+
+    /*create subscriber*/
+    lowstate_subscriber.reset(new ChannelSubscriber<unitree_go::msg::dds_::LowState_>(TOPIC_LOWSTATE));
+    lowstate_subscriber->InitChannel(std::bind(&Custom::LowStateMessageHandler, this, std::placeholders::_1), 1);
+
+    /*loop publishing thread*/
+    lowCmdWriteThreadPtr = CreateRecurrentThreadEx("writebasiccmd", UT_CPU_ID_NONE, 2000, &Custom::LowCmdWrite, this);
+}
+
+void Custom::InitLowCmd()
+{
+    low_cmd.head()[0] = 0xFE;
+    low_cmd.head()[1] = 0xEF;
+    low_cmd.level_flag() = 0xFF;
+    low_cmd.gpio() = 0;
+
+    for(int i=0; i<20; i++)
+    {
+        low_cmd.motor_cmd()[i].mode() = (0x01);   // motor switch to servo (PMSM) mode
+        low_cmd.motor_cmd()[i].q() = (PosStopF);
+        low_cmd.motor_cmd()[i].kp() = (0);
+        low_cmd.motor_cmd()[i].dq() = (VelStopF);
+        low_cmd.motor_cmd()[i].kd() = (0);
+        low_cmd.motor_cmd()[i].tau() = (0);
+    }
+}
+
+void Custom::InitRobotStateClient()
+{
+    rsc.SetTimeout(10.0f); 
+    rsc.Init();
+}
+
+int Custom::queryServiceStatus(const std::string& serviceName)
+{
+    std::vector<ServiceState> serviceStateList;
+    int ret,serviceStatus;
+    ret = rsc.ServiceList(serviceStateList);
+    size_t i, count=serviceStateList.size();
+    for (i=0; i<count; i++)
+    {
+        const ServiceState& serviceState = serviceStateList[i];
+        if(serviceState.name == serviceName)
+        {
+            if(serviceState.status == 0)
+            {
+                std::cout << "name: " << serviceState.name <<" is activate"<<std::endl;
+                serviceStatus = 1;
+            }
+            else
+            {
+                std::cout << "name:" << serviceState.name <<" is deactivate"<<std::endl;
+                serviceStatus = 0;
+            } 
+        }    
+    }
+    return serviceStatus;
+
+}
+
+void Custom::activateService(const std::string& serviceName,int activate)
+{
+    rsc.ServiceSwitch(serviceName, activate);  
+}
+
+void Custom::LowStateMessageHandler(const void* message)
+{
+    low_state = *(unitree_go::msg::dds_::LowState_*)message;
+}
+
+void Custom::LowCmdWrite()
+{
+    if(_percent_4<1)
+    {
+        std::cout<<"Read sensor data example: "<<std::endl;
+        std::cout<<"Joint 0 pos: "<<low_state.motor_state()[0].q()<<std::endl;
+        std::cout<<"Imu accelerometer : "<<"x: "<<low_state.imu_state().accelerometer()[0]<<" y: "<<low_state.imu_state().accelerometer()[1]<<" z: "<<low_state.imu_state().accelerometer()[2]<<std::endl;
+        std::cout<<"Foot force "<<low_state.foot_force()[0]<<std::endl;
+        std::cout<<std::endl;
+    }
+    if((_percent_4 == 1) && ( done == false))
+    {
+        std::cout<<"The example is done! "<<std::endl;
+        std::cout<<std::endl;
+        done = true;
+    }
+
+    motiontime++;
+    if(motiontime>=500)
+    {
+        if(firstRun)
+        {
+            for(int i = 0; i < 12; i++)
+            {
+                _startPos[i] = low_state.motor_state()[i].q();
+            }
+            firstRun = false;
+        }
+
+        _percent_1 += (float)1 / _duration_1;
+        _percent_1 = _percent_1 > 1 ? 1 : _percent_1;
+        if (_percent_1 < 1)
+        {
+            for (int j = 0; j < 12; j++)
+            {
+                low_cmd.motor_cmd()[j].q() = (1 - _percent_1) * _startPos[j] + _percent_1 * _targetPos_1[j];
+                low_cmd.motor_cmd()[j].dq() = 0;
+                low_cmd.motor_cmd()[j].kp() = Kp;
+                low_cmd.motor_cmd()[j].kd() = Kd;
+                low_cmd.motor_cmd()[j].tau() = 0;
+            }
+
+        }
+        if ((_percent_1 == 1)&&(_percent_2 < 1))
+        {
+            _percent_2 += (float)1 / _duration_2;
+            _percent_2 = _percent_2 > 1 ? 1 : _percent_2;
+
+            for (int j = 0; j < 12; j++)
+            {
+                low_cmd.motor_cmd()[j].q() = (1 - _percent_2) * _targetPos_1[j] + _percent_2 * _targetPos_2[j];
+                low_cmd.motor_cmd()[j].dq() = 0;
+                low_cmd.motor_cmd()[j].kp() = Kp;
+                low_cmd.motor_cmd()[j].kd() = Kd;
+                low_cmd.motor_cmd()[j].tau() = 0;
+            }
+        }
+
+        if ((_percent_1 == 1)&&(_percent_2 == 1)&&(_percent_3<1))
+        {
+            _percent_3 += (float)1 / _duration_3;
+            _percent_3 = _percent_3 > 1 ? 1 : _percent_3;
+
+            for (int j = 0; j < 12; j++)
+            {
+                low_cmd.motor_cmd()[j].q() =  _targetPos_2[j];
+                low_cmd.motor_cmd()[j].dq() = 0;
+                low_cmd.motor_cmd()[j].kp() = Kp;
+                low_cmd.motor_cmd()[j].kd() = Kd;
+                low_cmd.motor_cmd()[j].tau() = 0;
+            }
+        }
+        if ((_percent_1 == 1)&&(_percent_2 == 1)&&(_percent_3==1)&&((_percent_4<=1)))
+        {
+            _percent_4 += (float)1 / _duration_4;
+            _percent_4 = _percent_4 > 1 ? 1 : _percent_4;
+            for (int j = 0; j < 12; j++)
+            {
+                low_cmd.motor_cmd()[j].q() = (1 - _percent_4) * _targetPos_2[j] + _percent_4 * _targetPos_3[j];
+                low_cmd.motor_cmd()[j].dq() = 0;
+                low_cmd.motor_cmd()[j].kp() = Kp;
+                low_cmd.motor_cmd()[j].kd() = Kd;
+                low_cmd.motor_cmd()[j].tau() = 0;
+            }
+        }
+        low_cmd.crc() = crc32_core((uint32_t *)&low_cmd, (sizeof(unitree_go::msg::dds_::LowCmd_)>>2)-1);
+
+        lowcmd_publisher->Write(low_cmd);
+    }
+
+}
+
+int main(int argc, const char** argv)
+{
+    if (argc < 2)
+    {
+        std::cout << "Usage: " << argv[0] << " networkInterface" << std::endl;
+        exit(-1); 
+    }
+
+    std::cout << "WARNING: Make sure the robot is hung up or lying on the ground." << std::endl
+            << "Press Enter to continue..." << std::endl;
+    std::cin.ignore();
+
+    ChannelFactory::Instance()->Init(0, argv[1]);
+
+    Custom custom;
+    custom.InitRobotStateClient();
+    while(custom.queryServiceStatus("sport_mode"))
+    {
+        std::cout<<"Try to deactivate the service: "<<"sport_mode"<<std::endl;
+        custom.activateService("sport_mode",0);
+        sleep(1);
+    }
+
+    return 0;
+}

python/CMakeLists.txt

@@ -0,0 +1,25 @@
+project(crc_module)
+cmake_minimum_required(VERSION 3.5)
+
+find_package(pybind11 REQUIRED PATHS ${PYBIND_PATH}/pybind11/share/cmake/pybind11 NO_DEFAULT_PATH)
+
+pybind11_add_module(crc_module crc_pybind.cpp)
+include_directories(/usr/local/include/ddscxx /usr/local/include/iceoryx/v2.0.2)
+include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../include)
+include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/include)
+include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/include/ddscxx)
+link_libraries(unitree_sdk2 ddsc ddscxx rt pthread)
+
+# Link pybind11 to your module
+target_link_libraries(crc_module PRIVATE pybind11::module)
+
+# Disable symbol visibility for other targets
+set_target_properties(crc_module PROPERTIES CXX_VISIBILITY_PRESET "hidden")
+
+# Optional: set the output directory for the built module
+set_target_properties(crc_module PROPERTIES LIBRARY_OUTPUT_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}")
+
+set_target_properties(crc_module PROPERTIES PREFIX "")
+
+# Set C++ standard, pybind11 requires C++14 or later
+set_property(TARGET crc_module PROPERTY CXX_STANDARD 14)

python/README.md

@@ -0,0 +1,17 @@
+# Usage
+
+The current `crc_module.so` is compiled on arm64 cpu and python3.8 (compatible with the default Go2 environment). Please compile the library yourself if you are running on a different platform/python environment.
+
+To use it in your Python script (both sdk and ros), please put this file to a directory included in `$PYTHONPATH` and refer to `test_crc.py` for an example. You can also put `crc_module.pyi` to the same directory as `crc_module.so` to enable python type hint.
+
+# Compiling
+
+Activate the correct python environment and use `pip install pybind11` to install pybind.
+Find the pybind cmake path using `pip show pybind11 | grep Location | cut -d' ' -f2` and pass it to CMakeLists.txt
+
+```bash
+cd python
+mkdir build && cd build
+cmake .. -DPYBIND_PATH=$(pip show pybind11 | grep Location | cut -d' ' -f2)
+make
+```

python/crc_module.pyi

@@ -0,0 +1,4 @@
+from unitree_go.msg import LowCmd
+def get_crc(LowCmd: LowCmd) -> int:
+    """Calculates the CRC of a LowCmd message. Same as the C++ function."""
+    ...