Question on calculate A Matrix

[lgkimjy]

Dear @ShuoYangRobotics ,

In ConvexMPC.cpp, I do not understand why did you set value 1 at line 129, according to the ConvexMPC on MIT Cheetah3 paper, isn't it should be zero?

A1-QP-MPC-Controller/src/a1_cpp/src/ConvexMpc.cpp

Lines 110 to 130 in 79c9130

	void ConvexMpc::calculate_A_mat_c(Eigen::Vector3d root_euler) {
	// std::cout << "yaw: " << root_euler[2] << std::endl;
	double cos_yaw = cos(root_euler[2]);
	double sin_yaw = sin(root_euler[2]);
	// double cos_pitch = cos(root_euler[1]);
	// double tan_pitch = tan(root_euler[1]);
	Eigen::Matrix3d ang_vel_to_rpy_rate;
	// ang_vel_to_rpy_rate << cos_yaw / cos_pitch, sin_yaw / cos_pitch, 0,
	// -sin_yaw, cos_yaw, 0,
	// cos_yaw * tan_pitch, sin_yaw * tan_pitch, 1;
	ang_vel_to_rpy_rate << cos_yaw, sin_yaw, 0,
	-sin_yaw, cos_yaw, 0,
	0, 0, 1;
	A_mat_c.block<3, 3>(0, 6) = ang_vel_to_rpy_rate;
	A_mat_c.block<3, 3>(3, 9) = Eigen::Matrix3d::Identity();
	A_mat_c(11, NUM_DOF) = 1;
	}

can you explain why you put 1 in line 129?

[jbwenjoy]

I think this is because of the gravity term. The cheetah paper described this at the end of section III that they added an additional gravity state to put the dynamics into state-space form. Here the MPC_STATE_DIM is also 13 instead of 12.

Please correct me if I'm wrong.