EmbodiedAI
/
unitree_mujoco
mirror of https://github.com/unitreerobotics/unitree_mujoco.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

send rpy back in low state

This commit is contained in:
LuoBin 2025-03-05 11:20:05 +08:00
parent 3b3dfea0c8
commit f436261e7d
1 changed files with 26 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
simulate/src/unitree_sdk2_bridge/unitree_sdk2_bridge.cc
View File

@ -1,3 +1,5 @@
#include <eigen3/Eigen/Eigen>
#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];