which paper should we reference for "QuadKD::computeInverseDynamic()" method

[LittleDargon]

Hi,there,When I read the code QuadKD::computeInverseDynamic() function at quad_kd.cpp in quad_utils package, i find this inverse dynamic solved by a svd method through struct a Matrix which named "blk_mat", After my derivation, I can't understand how this part is to solve the inverse dynamics,and what is the principle of matrix construction about "blk_mat".

  Eigen::MatrixXd blk_mat =
      Eigen::MatrixXd::Zero(18 + constraints_num, 18 + constraints_num);
  blk_mat.block(0, 0, 6, 6) =
      -M.block(0, 0, 6, 6) +
      M.block(0, 6, 6, 12) * jacobian_inv * jacobian.block(0, 0, 12, 6);
  blk_mat.block(6, 0, 12, 6) =
      -M.block(6, 0, 12, 6) +
      M.block(6, 6, 12, 12) * jacobian_inv * jacobian.block(0, 0, 12, 6);
  for (size_t i = 0; i < 4; i++) {
    if (!contact_mode.at(leg_idx_list_.at(i))) {
      blk_mat.block(3 * i + 6, 3 * i + 6, 3, 3).diagonal().fill(1);
    }
  }
  blk_mat.block(0, 18, 18, constraints_num) = -A.transpose();
  blk_mat.block(18, 0, constraints_num, 6) =
      -A.leftCols(6) +
      A.rightCols(12) * jacobian_inv * jacobian.block(0, 0, 12, 6);

  // Perform inverse dynamics
  Eigen::VectorXd tau_swing(12), blk_sol(18 + constraints_num),
      blk_vec(18 + constraints_num);
  blk_vec.segment(0, 6) << N.segment(0, 6) + M.block(0, 6, 6, 12) *
                                                 jacobian_inv * foot_acc_q_ddot;
  blk_vec.segment(6, 12) << N.segment(6, 12) +
                                M.block(6, 6, 12, 12) * jacobian_inv *
                                    foot_acc_q_ddot -
                                tau_stance.segment(6, 12);
  blk_vec.segment(18, constraints_num)
      << A_dotq_dot + A.leftCols(12) * jacobian_inv * foot_acc_q_ddot;
  blk_sol =
      blk_mat.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(blk_vec);
  tau_swing = blk_sol.segment(6, 12);

Could you recommand some papers or books to help me know about this method

[jcnorby]

Hi there, glad you're interested in the repo! We actually got this question via private email a few weeks ago so it seems we should probably clarify this bit.

We typically follow A Mathematical Introduction to Robotic Manipulation by Murray, Li, and Sastry (MLS) for most of our math. That said, you are right to be slightly confused by the implementation as it doesn't directly follow MLS or any existing texts (that we know of). The best reference text is Legged Self-Manipulation, which reformulates some familiar MLS equations for legged systems. Take a look at Eq. (32) - (33), which express the dynamics in two different ways (the latter defines the block matrix which is inverted).

That text may help you parse the meaning of each term, although it doesn't contain this particular algorithm. We've found that inverse dynamics is a game of diminishing returns - adding more complexity may make performance slightly better, but there are typically other areas where development time will reap more benefits. The result is something of middling complexity - we leverage the assumption of massless legs when computing stance torques (yielding tau = -J^T*f), but in swing this assumption is obviously problematic. Instead of computing ID for the whole system (which I suppose we could do but haven't gotten around to yet), we use these terms to compute the predicted body acceleration from which we can derive the acceleration of the swing foot relative to the base of the leg link, and use ID to compute the resulting joint torques (the blk_mat and blk_vec terms are just used to compute these quantities).

So overall it differs because we are trying to leverage some simplifications to avoid doing whole body ID (mostly just for development time reasons), but without relying entirely on massless leg assumptions. Your mileage may vary - in some instances we removed this term because it seemed irrelevant, others may want to add their own whole body ID controller because they are a lot about swing leg inertia. That's the intent of this repo - you are free to use or modify as much as you like. Hope that helps!

[aaronjoh]

I'll add a pointer to A hybrid systems model for simple manipulation and self-manipulation systems where I go through the massless leg assumption and the block matrix inverse in more detail.

[LittleDargon]

I'll add a pointer to A hybrid systems model for simple manipulation and self-manipulation systems where I go through the massless leg assumption and the block matrix inverse in more detail.

Thanks for your reply

[LittleDargon]

Hi there, glad you're interested in the repo! We actually got this question via private email a few weeks ago so it seems we should probably clarify this bit.

We typically follow A Mathematical Introduction to Robotic Manipulation by Murray, Li, and Sastry (MLS) for most of our math. That said, you are right to be slightly confused by the implementation as it doesn't directly follow MLS or any existing texts (that we know of). The best reference text is Legged Self-Manipulation, which reformulates some familiar MLS equations for legged systems. Take a look at Eq. (32) - (33), which express the dynamics in two different ways (the latter defines the block matrix which is inverted).

That text may help you parse the meaning of each term, although it doesn't contain this particular algorithm. We've found that inverse dynamics is a game of diminishing returns - adding more complexity may make performance slightly better, but there are typically other areas where development time will reap more benefits. The result is something of middling complexity - we leverage the assumption of massless legs when computing stance torques (yielding tau = -J^T*f), but in swing this assumption is obviously problematic. Instead of computing ID for the whole system (which I suppose we could do but haven't gotten around to yet), we use these terms to compute the predicted body acceleration from which we can derive the acceleration of the swing foot relative to the base of the leg link, and use ID to compute the resulting joint torques (the blk_mat and blk_vec terms are just used to compute these quantities).

So overall it differs because we are trying to leverage some simplifications to avoid doing whole body ID (mostly just for development time reasons), but without relying entirely on massless leg assumptions. Your mileage may vary - in some instances we removed this term because it seemed irrelevant, others may want to add their own whole body ID controller because they are a lot about swing leg inertia. That's the intent of this repo - you are free to use or modify as much as you like. Hope that helps!

Thanks for your reply, it helped me a lot, I will go through the documentation you mentioned