Error in Gazebo example

[Kashu7100]

When running the Gazebo example, I got the following error in a1_cpp_ctrl_image. The build had no problems.

root@URANUS:~/A1_ctrl_ws# roslaunch a1_cpp a1_ctrl.launch type:=gazebo solver_type:=mpc
... logging to /root/.ros/log/d7c301c8-37e7-11ed-ae13-5800e38f14b5/roslaunch-URANUS-692.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://localhost:46479/

SUMMARY
========

PARAMETERS
 * /a1_default_foot_pos_FL_x: 0.17
 * /a1_default_foot_pos_FL_y: 0.15
 * /a1_default_foot_pos_FL_z: -0.35
 * /a1_default_foot_pos_FR_x: 0.17
 * /a1_default_foot_pos_FR_y: -0.15
 * /a1_default_foot_pos_FR_z: -0.35
 * /a1_default_foot_pos_RL_x: -0.17
 * /a1_default_foot_pos_RL_y: 0.15
 * /a1_default_foot_pos_RL_z: -0.35
 * /a1_default_foot_pos_RR_x: -0.17
 * /a1_default_foot_pos_RR_y: -0.15
 * /a1_default_foot_pos_RR_z: -0.35
 * /a1_gait_counter_speed_FL: 1.5
 * /a1_gait_counter_speed_FR: 1.5
 * /a1_gait_counter_speed_RL: 1.5
 * /a1_gait_counter_speed_RR: 1.5
 * /a1_kd_foot_x: 10.0
 * /a1_kd_foot_y: 10.0
 * /a1_kd_foot_z: 5.0
 * /a1_km_foot_x: 0.1
 * /a1_km_foot_y: 0.1
 * /a1_km_foot_z: 0.1
 * /a1_kp_foot_x: 200.0
 * /a1_kp_foot_y: 200.0
 * /a1_kp_foot_z: 150.0
 * /a1_robot_mass: 12.0
 * /a1_trunk_inertia_xx: 0.0158533
 * /a1_trunk_inertia_xy: 0.0
 * /a1_trunk_inertia_xz: 0.0
 * /a1_trunk_inertia_yy: 0.0377999
 * /a1_trunk_inertia_yz: 0.0
 * /a1_trunk_inertia_zz: 0.0456542
 * /q_weights_0: 20.0
 * /q_weights_10: 30.0
 * /q_weights_11: 10.0
 * /q_weights_12: 0.0
 * /q_weights_1: 10.0
 * /q_weights_2: 1.0
 * /q_weights_3: 0.0
 * /q_weights_4: 0.0
 * /q_weights_5: 420.0
 * /q_weights_6: 0.05
 * /q_weights_7: 0.05
 * /q_weights_8: 0.05
 * /q_weights_9: 30.0
 * /r_weights_0: 1e-7
 * /r_weights_10: 1e-7
 * /r_weights_11: 1e-7
 * /r_weights_1: 1e-7
 * /r_weights_2: 1e-7
 * /r_weights_3: 1e-7
 * /r_weights_4: 1e-7
 * /r_weights_5: 1e-7
 * /r_weights_6: 1e-7
 * /r_weights_7: 1e-7
 * /r_weights_8: 1e-7
 * /r_weights_9: 1e-7
 * /rosdistro: melodic
 * /rosversion: 1.14.13
 * /stance_leg_control_type: 1
 * /use_sim_time: True

NODES
  /
    gazebo_a1_ctrl (a1_cpp/gazebo_a1_ctrl)

ROS_MASTER_URI=http://localhost:11311

process[gazebo_a1_ctrl-1]: started with pid [701]
init A1RobotControl
init A1RobotControl
init nh
Enter thread 1: compute desired ground forces
Enter thread 2: update robot states, compute desired swing legs forces, compute desired joint torques, and send commands
foot_pos_recent_contact z: 0 0 0 0
[DEBUG] [1663570540.605644433, 24.637500000]: Trying to publish message of type [nav_msgs/Odometry/cd5e73d190d741a2f92e81eda573aca7] on a publisher with type [nav_msgs/Odometry/cd5e73d190d741a2f92e81eda573aca7]
[DEBUG] [1663570540.606794327, 24.637600000]: Trying to publish message of type [std_msgs/Float64/fdb28210bfa9d7c91146260178d9a584] on a publisher with type [std_msgs/Float64/fdb28210bfa9d7c91146260178d9a584]
desire pitch in deg: 0
terrain angle: 0
[DEBUG] [1663570540.606865943, 24.637600000]: Trying to publish message of type [unitree_legged_msgs/MotorCmd/bbb3b7d91319c3a1b99055f0149ba221] on a publisher with type [unitree_legged_msgs/MotorCmd/bbb3b7d91319c3a1b99055f0149ba221]
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.
[gazebo_a1_ctrl-1] process has died [pid 701, exit code -11, cmd /root/A1_ctrl_ws/devel/lib/a1_cpp/gazebo_a1_ctrl __name:=gazebo_a1_ctrl __log:=/root/.ros/log/d7c301c8-37e7-11ed-ae13-5800e38f14b5/gazebo_a1_ctrl-1.log].
log file: /root/.ros/log/d7c301c8-37e7-11ed-ae13-5800e38f14b5/gazebo_a1_ctrl-1*.log
all processes on machine have died, roslaunch will exit
shutting down processing monitor...
... shutting down processing monitor complete
done

Do you have any idea what is causing this error?

[P1terQ]

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

[Kashu7100]

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?

[YinghaoJia98]

@P1terQ Add an enormous number of points to USTC!

[Shivam7Sharma]

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

I copy pasted the code in the imu_callback function and did catkin build, but I am still getting the same error. This didn't help. The error only happens when I simulate the robot in the same container as the controller.