Comments (4)
i met this problem before. you can try to ros echo /torso_odom
,see if there is anything like orientation.w odom->pose.pose.orientation.x ...
. If not, then you probably meet the same problem as me, RotationMatrix become 0 in Matrix A. You need to copy the functions in gt_pose_callback
to imu_callback
. After that, the imu_callback
should look like this:
void GazeboA1ROS::imu_callback(const sensor_msgs::Imu::ConstPtr &imu) {
a1_ctrl_states.root_quat = Eigen::Quaterniond(quat_w.CalculateAverage(imu->orientation.w),
quat_x.CalculateAverage(imu->orientation.x),
quat_y.CalculateAverage(imu->orientation.y),
quat_z.CalculateAverage(imu->orientation.z));
a1_ctrl_states.root_rot_mat = a1_ctrl_states.root_quat.toRotationMatrix();
a1_ctrl_states.imu_acc = Eigen::Vector3d(
acc_x.CalculateAverage(imu->linear_acceleration.x),
acc_y.CalculateAverage(imu->linear_acceleration.y),
acc_z.CalculateAverage(imu->linear_acceleration.z)
);
a1_ctrl_states.imu_ang_vel = Eigen::Vector3d(
gyro_x.CalculateAverage(imu->angular_velocity.x),
gyro_y.CalculateAverage(imu->angular_velocity.y),
gyro_z.CalculateAverage(imu->angular_velocity.z)
);
a1_ctrl_states.root_ang_vel = a1_ctrl_states.root_rot_mat * a1_ctrl_states.imu_ang_vel;
a1_ctrl_states.root_euler = Utils::quat_to_euler(a1_ctrl_states.root_quat);
double yaw_angle = a1_ctrl_states.root_euler[2];
a1_ctrl_states.root_rot_mat_z = Eigen::AngleAxisd(yaw_angle, Eigen::Vector3d::UnitZ());
// FL, FR, RL, RR
for (int i = 0; i < NUM_LEG; ++i) {
a1_ctrl_states.foot_pos_rel.block<3, 1>(0, i) = a1_kin.fk(
a1_ctrl_states.joint_pos.segment<3>(3 * i),
rho_opt_list[i], rho_fix_list[i]);
a1_ctrl_states.j_foot.block<3, 3>(3 * i, 3 * i) = a1_kin.jac(
a1_ctrl_states.joint_pos.segment<3>(3 * i),
rho_opt_list[i], rho_fix_list[i]);
Eigen::Matrix3d tmp_mtx = a1_ctrl_states.j_foot.block<3, 3>(3 * i, 3 * i);
Eigen::Vector3d tmp_vec = a1_ctrl_states.joint_vel.segment<3>(3 * i);
a1_ctrl_states.foot_vel_rel.block<3, 1>(0, i) = tmp_mtx * tmp_vec;
a1_ctrl_states.foot_pos_abs.block<3, 1>(0, i) = a1_ctrl_states.root_rot_mat * a1_ctrl_states.foot_pos_rel.block<3, 1>(0, i);
a1_ctrl_states.foot_vel_abs.block<3, 1>(0, i) = a1_ctrl_states.root_rot_mat * a1_ctrl_states.foot_vel_rel.block<3, 1>(0, i);
a1_ctrl_states.foot_pos_world.block<3, 1>(0, i) = a1_ctrl_states.foot_pos_abs.block<3, 1>(0, i) + a1_ctrl_states.root_pos;
a1_ctrl_states.foot_vel_world.block<3, 1>(0, i) = a1_ctrl_states.foot_vel_abs.block<3, 1>(0, i) + a1_ctrl_states.root_lin_vel;
}
}
I'm not sure what causes the problem. I guess maybe it's the unitree sdk version
from a1-qp-mpc-controller.
I guess you are right; it seems there is some problem related to /torso_odom
. I will look into the problem.
Here is my output for rostopic echo /torso_odom
. Note that rostopic echo /trunk_imu
works fine.
root@URANUS:~/A1_ctrl_ws# rostopic echo /torso_odom
WARNING: no messages received and simulated time is active.
Is /clock being published?
from a1-qp-mpc-controller.
Related Issues (20)
- Consideration of using LCM? HOT 3
- why 4th order Bezier Curves is used instead of 3rd order, like MIT? HOT 1
- static error with QP control when trying to stand still HOT 2
- Is there any way to get body accelerations from Isaac Sim?
- Possible missing coefficient when converting QP controller to osqp. HOT 1
- On the Constraint Problem of Friction Cone
- roslaunch报错:ERROR in LDL_factor: Error in KKT matrix LDL factorization when computing the nonzero elements. The problem seems to be non-convex ERROR in osqp_setup: KKT matrix factorization. The problem seems to be non-convex. [OsqpEigen::Solver::initSolver] Unable to setup the workspace. [OsqpEigen::Solver::solveProblem] The solve has not been initialized yet. Please call initSolver() method. [OsqpEigen::Solver::solve] Unable to solve the problem. HOT 1
- PATH_OF_THE_REPO_ON_YOUR_HOST_COMPUTER HOT 2
- a1关节位置获取延迟 HOT 1
- Major problems in robot param & MPC control HOT 1
- Error catkin_make
- localhost port 2233: Connection refused HOT 2
- Controller does not continue/no topics from gazebo received
- euler error and PD law for QP formulation
- 梁亭演示
- 有在AMD显卡上成功运行的吗?
- Init-Compilation Failure Issue HOT 2
- Revised MPC Controller
- does your controller need foot sensors ? Or can I do without them ?!
Recommend Projects
-
React
A declarative, efficient, and flexible JavaScript library for building user interfaces.
-
Vue.js
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
-
Typescript
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
-
TensorFlow
An Open Source Machine Learning Framework for Everyone
-
Django
The Web framework for perfectionists with deadlines.
-
Laravel
A PHP framework for web artisans
-
D3
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
-
Recommend Topics
-
javascript
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
-
web
Some thing interesting about web. New door for the world.
-
server
A server is a program made to process requests and deliver data to clients.
-
Machine learning
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
-
Visualization
Some thing interesting about visualization, use data art
-
Game
Some thing interesting about game, make everyone happy.
Recommend Org
-
Facebook
We are working to build community through open source technology. NB: members must have two-factor auth.
-
Microsoft
Open source projects and samples from Microsoft.
-
Google
Google ❤️ Open Source for everyone.
-
Alibaba
Alibaba Open Source for everyone
-
D3
Data-Driven Documents codes.
-
Tencent
China tencent open source team.
from a1-qp-mpc-controller.