From c38f583e89632dd9b586fb42ddd6e205be91cf27 Mon Sep 17 00:00:00 2001 From: LuoBin <2646887443@qq.com> Date: Wed, 12 Mar 2025 12:58:56 +0800 Subject: [PATCH] don't use eigen --- .../unitree_sdk2_bridge.cc | 36 ++++++++----------- 1 file changed, 14 insertions(+), 22 deletions(-) diff --git a/simulate/src/unitree_sdk2_bridge/unitree_sdk2_bridge.cc b/simulate/src/unitree_sdk2_bridge/unitree_sdk2_bridge.cc index 4c9139c..644d576 100644 --- a/simulate/src/unitree_sdk2_bridge/unitree_sdk2_bridge.cc +++ b/simulate/src/unitree_sdk2_bridge/unitree_sdk2_bridge.cc @@ -1,5 +1,3 @@ -#include - #include "unitree_sdk2_bridge.h" UnitreeSdk2Bridge::UnitreeSdk2Bridge(mjModel *model, mjData *data) : mj_model_(model), mj_data_(data) @@ -88,17 +86,14 @@ void UnitreeSdk2Bridge::PublishLowStateGo() low_state_go_.imu_state().quaternion()[2] = mj_data_->sensordata[dim_motor_sensor_ + 2]; low_state_go_.imu_state().quaternion()[3] = mj_data_->sensordata[dim_motor_sensor_ + 3]; - Eigen::Quaternionf quat; - quat.w() = low_state_go_.imu_state().quaternion()[0]; - quat.x() = low_state_go_.imu_state().quaternion()[1]; - quat.y() = low_state_go_.imu_state().quaternion()[2]; - quat.z() = low_state_go_.imu_state().quaternion()[3]; + double w = low_state_go_.imu_state().quaternion()[0]; + double x = low_state_go_.imu_state().quaternion()[1]; + double y = low_state_go_.imu_state().quaternion()[2]; + double z = low_state_go_.imu_state().quaternion()[3]; - Eigen::Vector3f ypr = quat.toRotationMatrix().canonicalEulerAngles(2, 1, 0); - - low_state_go_.imu_state().rpy()[0] = ypr[2]; - low_state_go_.imu_state().rpy()[1] = ypr[1]; - low_state_go_.imu_state().rpy()[2] = ypr[0]; + low_state_go_.imu_state().rpy()[0] = atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y)); + low_state_go_.imu_state().rpy()[1] = asin(2 * (w * y - z * x)); + low_state_go_.imu_state().rpy()[2] = atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z)); low_state_go_.imu_state().gyroscope()[0] = mj_data_->sensordata[dim_motor_sensor_ + 4]; low_state_go_.imu_state().gyroscope()[1] = mj_data_->sensordata[dim_motor_sensor_ + 5]; @@ -137,17 +132,14 @@ void UnitreeSdk2Bridge::PublishLowStateHg() low_state_hg_.imu_state().quaternion()[2] = mj_data_->sensordata[dim_motor_sensor_ + 2]; low_state_hg_.imu_state().quaternion()[3] = mj_data_->sensordata[dim_motor_sensor_ + 3]; - Eigen::Quaternionf quat; - quat.w() = low_state_hg_.imu_state().quaternion()[0]; - quat.x() = low_state_hg_.imu_state().quaternion()[1]; - quat.y() = low_state_hg_.imu_state().quaternion()[2]; - quat.z() = low_state_hg_.imu_state().quaternion()[3]; + double w = low_state_hg_.imu_state().quaternion()[0]; + double x = low_state_hg_.imu_state().quaternion()[1]; + double y = low_state_hg_.imu_state().quaternion()[2]; + double z = low_state_hg_.imu_state().quaternion()[3]; - Eigen::Vector3f ypr = quat.toRotationMatrix().canonicalEulerAngles(2, 1, 0); - - low_state_hg_.imu_state().rpy()[0] = ypr[2]; - low_state_hg_.imu_state().rpy()[1] = ypr[1]; - low_state_hg_.imu_state().rpy()[2] = ypr[0]; + low_state_hg_.imu_state().rpy()[0] = atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y)); + low_state_hg_.imu_state().rpy()[1] = asin(2 * (w * y - z * x)); + low_state_hg_.imu_state().rpy()[2] = atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z)); low_state_hg_.imu_state().gyroscope()[0] = mj_data_->sensordata[dim_motor_sensor_ + 4]; low_state_hg_.imu_state().gyroscope()[1] = mj_data_->sensordata[dim_motor_sensor_ + 5];