add g1 protect functions

Author: Agnel-Wang

example/g1/CMakeLists.txt

@@ -1,4 +1,3 @@
-
 add_executable(g1_loco_client high_level/g1_loco_client_example.cpp)
 target_link_libraries(g1_loco_client unitree_sdk2)
 
@@ -20,6 +19,11 @@ target_link_libraries(g1_audio_client_example unitree_sdk2)
 add_executable(g1_dex3_example dex3/g1_dex3_example.cpp)
 target_link_libraries(g1_dex3_example unitree_sdk2)
 
+find_package(Boost COMPONENTS program_options)
+if(Boost_FOUND)
+    add_executable(g1_termination low_level/terminations.cpp)
+    target_link_libraries(g1_termination unitree_sdk2 Boost::program_options)
+endif()
 
 find_package(yaml-cpp QUIET)
 if(yaml-cpp_FOUND)

example/g1/low_level/terminations.cpp

@@ -0,0 +1,46 @@
+#include <unitree/robot/g1/common/terminations.hpp>
+#include <boost/program_options.hpp>
+#include <thread>
+
+namespace po = boost::program_options;
+
+using namespace unitree::robot;
+using namespace unitree_hg::msg::dds_;
+
+int main(int argc, char** argv)
+{
+    // Parse command line arguments
+    po::options_description desc("Unitree G1 termination functions testing.");
+    desc.add_options()
+        ("network,n", po::value<std::string>()->default_value(""), "dds network interface")
+    ;
+    po::variables_map vm;
+    po::store(po::parse_command_line(argc, argv, desc), vm);
+    po::notify(vm);
+    std::cout << desc << std::endl;
+    
+    // DDS Init
+    ChannelFactory::Instance()->Init(0, vm["network"].as<std::string>());
+
+    auto lowstate_subscriber = std::make_shared<ChannelSubscriber<LowState_>>("rt/lowstate");
+    LowState_ lowstate;
+    lowstate_subscriber->InitChannel([&lowstate](const void* message) {
+        lowstate = *(const LowState_*)message;
+    });
+
+    std::cout << "Checking terminations..." << std::endl;
+
+    while (true)
+    {
+        if (g1::bad_orientation(lowstate, 1.0f)) { // Tip the robot over to test bad orientation
+            std::cout << "Bad orientation detected!" << std::endl;
+        }
+        if (g1::lost_connection(lowstate_subscriber, 1000)) { // Unplug the network cable to test lost connection
+            std::cout << "Lost connection!" << std::endl;
+        }
+
+        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    }
+    
+    return 0;
+}

include/unitree/robot/g1/common/terminations.hpp

@@ -0,0 +1,90 @@
+#pragma once
+
+/**
+ * This file implements some functions used to protect the G1 during runtime.
+ * When the function returns true, it is recommended to set the motor to passive mode in the lower-level control.
+ */
+
+#include <eigen3/Eigen/Dense>
+#include <unitree/idl/hg/LowState_.hpp>
+#include <unitree/idl/hg/BmsState_.hpp>
+#include <unitree/robot/channel/channel_subscriber.hpp>
+
+namespace unitree {
+namespace robot {
+namespace g1 {  
+
+// robot orientation is too far from the desired orientation limits.
+inline bool bad_orientation(const unitree_hg::msg::dds_::LowState_ & lowstate, float limit_angle = 1.0)
+{
+    auto & imu = lowstate.imu_state();
+    Eigen::Quaternionf quat(
+        imu.quaternion()[0],
+        imu.quaternion()[1],
+        imu.quaternion()[2],
+        imu.quaternion()[3]
+    );
+    Eigen::Vector3f projected_gravity_b = quat.conjugate() * Eigen::Vector3f(0, 0, -1);
+    return std::fabs(std::acos(-projected_gravity_b[2])) > limit_angle;
+}
+
+// Joint velocities are outside of the soft joint limits
+inline bool joint_vel_out_of_limit(const unitree_hg::msg::dds_::LowState_ & lowstate, float limit_vel = 10.0)
+{
+    auto & motors = lowstate.motor_state();
+    return std::any_of(motors.begin(), motors.end(), [limit_vel](const auto & motor) {
+        return std::fabs(motor.dq()) > limit_vel;
+    });
+}
+
+// Agnular velocities are outside of the soft joint limits
+inline bool ang_vel_out_of_limit(const unitree_hg::msg::dds_::LowState_ & lowstate, float limit_vel = 6.0)
+{
+    auto & gyroscope = lowstate.imu_state().gyroscope();
+    return std::any_of(gyroscope.begin(), gyroscope.end(), [limit_vel](const auto & ang_vel) {
+        return std::fabs(ang_vel) > limit_vel;
+    });
+}
+
+// Motor winding temperature is above the limit
+inline bool motor_winding_overheat(const unitree_hg::msg::dds_::LowState_ & lowstate, float limit_temp = 120.0)
+{
+    auto & motors = lowstate.motor_state();
+    return std::any_of(motors.begin(), motors.end(), [limit_temp](const auto & motor) {
+        return motor.temperature()[1] > limit_temp;
+    });
+}
+
+// Motor casing temperature is above the limit
+inline bool motor_casing_overheat(const unitree_hg::msg::dds_::LowState_ & lowstate, float limit_temp = 85.0)
+{
+    auto & motors = lowstate.motor_state();
+    return std::any_of(motors.begin(), motors.end(), [limit_temp](const auto & motor) {
+        return motor.temperature()[0] > limit_temp;
+    });
+}
+
+// State of charge (SOC) is below the limit 
+inline bool low_battery(const  unitree_hg::msg::dds_::BmsState_ & bms_state, float limit_soc = 20.0)
+{
+    return bms_state.soc() < limit_soc;
+}
+
+/**
+ * @brief Lost connection to the robot
+ * This function checks if the last data available time is older than the specified timeout.
+ * If the timeout is reached, it indicates a lost connection.
+ * 
+ * When using a wired connection to the robot, a loose network cable may cause the connection to be interrupted.
+ * If the program continues to run at this time, it will send a step signal to the motors, causing violent movement.
+ */
+inline bool lost_connection(unitree::robot::ChannelSubscriberPtr<unitree_hg::msg::dds_::LowState_> & subscriber, int64_t timeout_ms = 1000)
+{
+    auto now = unitree::common::GetCurrentMonotonicTimeNanosecond();
+    auto elasped_ms = (now - subscriber->GetLastDataAvailableTime()) / 1e6;
+    return elasped_ms > timeout_ms;
+}
+
+}
+}
+}