From f436261e7dafcfe311c686d45169c4bc1e250dfe Mon Sep 17 00:00:00 2001 From: LuoBin <2646887443@qq.com> Date: Wed, 5 Mar 2025 11:20:05 +0800 Subject: [PATCH] send rpy back in low state --- .../unitree_sdk2_bridge.cc | 26 +++++++++++++++++++ 1 file changed, 26 insertions(+) diff --git a/simulate/src/unitree_sdk2_bridge/unitree_sdk2_bridge.cc b/simulate/src/unitree_sdk2_bridge/unitree_sdk2_bridge.cc index 710f8a9..4c9139c 100644 --- a/simulate/src/unitree_sdk2_bridge/unitree_sdk2_bridge.cc +++ b/simulate/src/unitree_sdk2_bridge/unitree_sdk2_bridge.cc @@ -1,3 +1,5 @@ +#include + #include "unitree_sdk2_bridge.h" UnitreeSdk2Bridge::UnitreeSdk2Bridge(mjModel *model, mjData *data) : mj_model_(model), mj_data_(data) @@ -86,6 +88,18 @@ 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]; + + 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().gyroscope()[0] = mj_data_->sensordata[dim_motor_sensor_ + 4]; low_state_go_.imu_state().gyroscope()[1] = mj_data_->sensordata[dim_motor_sensor_ + 5]; low_state_go_.imu_state().gyroscope()[2] = mj_data_->sensordata[dim_motor_sensor_ + 6]; @@ -123,6 +137,18 @@ 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]; + + 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().gyroscope()[0] = mj_data_->sensordata[dim_motor_sensor_ + 4]; low_state_hg_.imu_state().gyroscope()[1] = mj_data_->sensordata[dim_motor_sensor_ + 5]; low_state_hg_.imu_state().gyroscope()[2] = mj_data_->sensordata[dim_motor_sensor_ + 6];