80 lines
2.5 KiB
C++
80 lines
2.5 KiB
C++
#pragma once
|
|
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
#include <cstdint>
|
|
|
|
#include <eigen3/Eigen/Dense>
|
|
|
|
#include <unitree/idl/hg/BmsState_.hpp>
|
|
#include <unitree/idl/hg/LowState_.hpp>
|
|
#include <unitree/robot/channel/channel_subscriber.hpp>
|
|
|
|
namespace unitree {
|
|
namespace robot {
|
|
namespace h2 {
|
|
|
|
inline bool bad_orientation(const unitree_hg::msg::dds_::LowState_& lowstate,
|
|
float limit_angle = 1.0f) {
|
|
const auto& imu = lowstate.imu_state();
|
|
Eigen::Quaternionf quat(imu.quaternion()[0], imu.quaternion()[1],
|
|
imu.quaternion()[2], imu.quaternion()[3]);
|
|
const Eigen::Vector3f projected_gravity_b =
|
|
quat.conjugate() * Eigen::Vector3f(0, 0, -1);
|
|
return std::fabs(std::acos(-projected_gravity_b[2])) > limit_angle;
|
|
}
|
|
|
|
inline bool joint_vel_out_of_limit(
|
|
const unitree_hg::msg::dds_::LowState_& lowstate,
|
|
float limit_vel = 10.0f) {
|
|
const 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;
|
|
});
|
|
}
|
|
|
|
inline bool ang_vel_out_of_limit(
|
|
const unitree_hg::msg::dds_::LowState_& lowstate,
|
|
float limit_vel = 6.0f) {
|
|
const auto& gyroscope = lowstate.imu_state().gyroscope();
|
|
return std::any_of(gyroscope.begin(), gyroscope.end(), [limit_vel](float value) {
|
|
return std::fabs(value) > limit_vel;
|
|
});
|
|
}
|
|
|
|
inline bool motor_winding_overheat(
|
|
const unitree_hg::msg::dds_::LowState_& lowstate,
|
|
float limit_temp = 120.0f) {
|
|
const auto& motors = lowstate.motor_state();
|
|
return std::any_of(motors.begin(), motors.end(), [limit_temp](const auto& motor) {
|
|
return motor.temperature()[1] > limit_temp;
|
|
});
|
|
}
|
|
|
|
inline bool motor_casing_overheat(
|
|
const unitree_hg::msg::dds_::LowState_& lowstate,
|
|
float limit_temp = 85.0f) {
|
|
const auto& motors = lowstate.motor_state();
|
|
return std::any_of(motors.begin(), motors.end(), [limit_temp](const auto& motor) {
|
|
return motor.temperature()[0] > limit_temp;
|
|
});
|
|
}
|
|
|
|
inline bool low_battery(const unitree_hg::msg::dds_::BmsState_& bms_state,
|
|
float limit_soc = 20.0f) {
|
|
return bms_state.soc() < limit_soc;
|
|
}
|
|
|
|
inline bool lost_connection(
|
|
unitree::robot::ChannelSubscriberPtr<unitree_hg::msg::dds_::LowState_>& subscriber,
|
|
int64_t timeout_ms = 1000) {
|
|
const auto now = unitree::common::GetCurrentMonotonicTimeNanosecond();
|
|
const auto elapsed_ms =
|
|
(now - subscriber->GetLastDataAvailableTime()) / 1e6;
|
|
return elapsed_ms > timeout_ms;
|
|
}
|
|
|
|
} // namespace h2
|
|
} // namespace robot
|
|
} // namespace unitree
|