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

don't use eigen

This commit is contained in:
LuoBin 2025-03-12 12:58:56 +08:00
parent f436261e7d
commit c38f583e89
1 changed files with 14 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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