慣性行列を計算する関数calcMassMatrixについて

kkaya · August 27, 2019, 2:52am

初めて質問させていただきます。
二足歩行ロボットの制御シミュレーションをChoreonoidを用いて行っています。その際、運動方程式内の慣性行列Mを取得する必要があるのですが、calcMassMatrix関数を用いて値を取得した場合、転置で同一になるような形で出力されませんでした。

浮遊リンク系をモデル化した場合、以下の論文(7-19式)のように、転置で同一になる形で出力されると考えていましたが、空間速度ベクトルを用いているため、従来の加速度、角加速度とは異なる形で慣性行列Mが出力されるのでしょうか。
それとも他にやり方があるのでしょうか。

参考：吉田和哉, The SpaceDyn: ダイナミクズ計算のためのMATLABツールボックス
[https://www.jstage.jst.go.jp/article/sicejl1962/38/2/38_2_138/_article/-char/ja]

ご回答いただけると幸いです。よろしくお願いいたします。

Yuki_Onishi · August 30, 2019, 3:36pm

実際の出力される行列と，ソースコードを載せていただくことはできますか？
こちらでも検証します．

kkaya · September 1, 2019, 6:47am

返信ありがとうございます。

実際にやりたいことは、各制御ループ内で慣性行列を取得することになります。
Simpleコントローラとして実装したコードを下記に添付いたします。
また、実際に使ったモデル(簡単のため、SR1モデルから上体を取り除いたモデル)のBODYファイルも添付いたします。

MyController.zip (1.1 KB)
SR1.body (7.3 KB)

実際に1ループ目で出力された慣性行列は以下の18×18行列になります。

よろしくお願いいたします。

Yuki_Onishi · September 3, 2019, 7:26am

ソースコードを拝見しました，こちらで試した結果同じ現象が確認できました．
実験的な回答で恐縮なのですが，下記の二重for文を追加実装することで慣性行列を算出できました．

//慣性行列の計算
calcMassMatrix(robot, MM);

for(uint8_t i = 1; i < MM.cols(); i++) {
    for(uint8_t j = 0; j < i && j < 3; j++) {
        MM.col(i) -= MM.col(j);
    }
}

cout << "慣性行列 M =" << endl;
cout << std::setprecision(2) << MM << endl;

今回観測されたバグ(?)としては，慣性行列の重心並進に対応する列 (0~2列目) が以降の列に全て加算されているようでした．
これを取り除いたところ，対称性のある，物理的にも無理のない値が出てきました．
（画像で添付します）

まだ内部実装を追いきれていないのですが，取り急ぎご確認いただければと思います．

Yuki_Onishi · September 3, 2019, 7:34am

内部実装されている場所は，こちらと思われます．

src/Body/MassMatrix.cpp

nakaoka · September 3, 2019, 2:23pm

kkaya様、不具合のご指摘ありがとうございます。
そしてYuki_Onishi様、修正方法について調査・提示いただき誠にありがとうございます。
こちらでも確認して修正を取り込みたいと思います。
（ただすみません、今会社の業務で立て込んでおりまして、今しばらくお待ちいただけますでしょうか。よろしくお願い致します。）

nakaoka · November 7, 2019, 9:21am

本件対応が大変遅くなりまして申し訳ありません。
バグの方修正してコミットしておきました。

kkaya さんのサンプルについても正常な値が出力されることを確認しました。

なお、この関数は以前はBodyヘッダをインクルードすると使えていましたが、必ずしもBodyクラスと同時に使うものではないため、Bodyヘッダに含めるのは見直しました。MassMatrixというヘッダがありますので、

#include <cnoid/MassMatrix>

としてお使いください。

バグの原因としては、単位ベクトル法で各加速度要素を順番に1にしていく中で、ルートリンクの並進要素に関して0に戻し忘れている部分がありました。単純なコーディングミスです。

Yuki_Onishi さんの調査・報告のおかげでこのバグをみつけることができました。ありがとうございました。

nakaoka · November 8, 2019, 12:41am

calcMassMatrix関数について、直したついでに必要な計算まで除去してしまっていたことに気がついたので、再度修正しておきました。

toruhishinuma · November 21, 2022, 12:56pm

初めまして、

先にGithubにIssueを立ててしまったのですが、その後にdiscouseの方が活発に議論が行われているとのことを知ったため、こちらでも質問させていただきます。

github.com/choreonoid/choreonoid

rotor inertia in mass matrix computation

opened 04:51AM - 17 Nov 22 UTC

th1991-01

Hi, In the mass matrix computation, rotor inertia seems added twice. Firstly…, as a part of computing mass matrix, the inverse dynamics computation to obtain `A_G * delta_i + b_G` considers rotor inertia, as seen at https://github.com/choreonoid/choreonoid/blob/master/src/Body/InverseDynamics.cpp#L66. Secondly, rotor inertia is also added at https://github.com/choreonoid/choreonoid/blob/master/src/Body/MassMatrix.cpp#L93. typo comment?: motor inertia -> rotor inertia at https://github.com/choreonoid/choreonoid/blob/master/src/Body/MassMatrix.cpp#L93.

calcMassMatrix関数を用いた慣性行列計算において、rotorInertiaが２回足されているように思います。まず、

github.com

choreonoid/choreonoid/blob/master/src/Body/MassMatrix.cpp#L92

    
      
                  setColumnOfMassMatrix(body, out_M, i + 3);
                  rootLink->dw()[i] = 0.0;
              }
          }
          
          
const int offset = rootLink->isFixedJoint() ? 0 : 6;
          for(int i = 0; i < nj; ++i){
              Link* joint = body->joint(i);
              joint->ddq() = 1.0;
              const int j = i + offset;
              setColumnOfMassMatrix(body, out_M, j);
              out_M(j, j) += joint->Jm2(); // motor inertia
              joint->ddq() = 0.0;
          }
          
          
// subtract the constant term
          for(int i = 0; i < out_M.cols(); ++i){
              out_M.col(i) -= out_b;
          }
          
          
// recover state

github.com

choreonoid/choreonoid/blob/master/src/Body/MassMatrix.cpp#L17

    
      
          #include "InverseDynamics.h"
          
          
using namespace cnoid;
          
          
namespace {
          
          
template<typename Derived>
          void setColumnOfMassMatrix(Body* body, Eigen::MatrixBase<Derived>& out_M, int column)
          {
              Link* rootLink = body->rootLink();
              Vector6 f = calcInverseDynamics(rootLink);
              int rowOffset;
              if(rootLink->isFixedJoint()){
                  rowOffset = 0;
              } else {
                  // for the floating root link
                  f.tail<3>() -= rootLink->p().cross(f.head<3>());
                  out_M. Eigen::template MatrixBase<Derived>::template block<6, 1>(0, column) = f;
                  rowOffset = 6;
              }
              const int n = body->numJoints();

の中でやっている逆動力学演算A_G * delta_i + b_G （梶田先生教科書第2版6.40式）

github.com

choreonoid/choreonoid/blob/master/src/Body/InverseDynamics.cpp#L66

    
      
          
          
    Vector6 f;
              f.head<3>() = link->m() * (dvo + link->dw().cross(c)) + link->w().cross(P);
              f.tail<3>() = link->m() * c.cross(dvo) + I * link->dw() + vo.cross(P) + link->w().cross(L);
          
          
    for(Link* child = link->child(); child; child = child->sibling()){
                  f += calcInverseDynamicsSub(child, vo, dvo);
              }
              f -= link->F_ext();
              
              link->u() = sv.dot(f.head<3>()) + sw.dot(f.tail<3>()) + link->ddq() * link->Jm2() /* rotor inertia */;
          
          
    return f;
          }
          
          
}
              
          namespace cnoid {
          
          
Vector6 calcInverseDynamics(Link* link)
          {

で、rotorInertiaを含んだ形でA_G * delta_i + b_G を計算しています。その後、

github.com

choreonoid/choreonoid/blob/master/src/Body/MassMatrix.cpp#L93

    
      
                  rootLink->dw()[i] = 0.0;
              }
          }
          
          
const int offset = rootLink->isFixedJoint() ? 0 : 6;
          for(int i = 0; i < nj; ++i){
              Link* joint = body->joint(i);
              joint->ddq() = 1.0;
              const int j = i + offset;
              setColumnOfMassMatrix(body, out_M, j);
              out_M(j, j) += joint->Jm2(); // motor inertia
              joint->ddq() = 0.0;
          }
          
          
// subtract the constant term
          for(int i = 0; i < out_M.cols(); ++i){
              out_M.col(i) -= out_b;
          }
          
          
// recover state
          for(int i = 0; i < nj; ++i){

で、A_G * delta_i + b_G のi成分にさらにrotorInertiaを足しています。

順動力学計算コード

github.com

choreonoid/choreonoid/blob/master/src/Body/ForwardDynamicsABM.cpp#L379

    
      
                          link->ptau() += uu_dd * child->hhw();
                      }
                  }
          
          
        if(i > 0){
                      if(!link->isFixedJoint()){
                          // hh = Ia * s
                          link->hhv().noalias() = link->Ivv() * link->sv() + link->Iwv().transpose() * link->sw();
                          link->hhw().noalias() = link->Iwv() * link->sv() + link->Iww() * link->sw();
                          // dd = Ia * s * s^T
                          link->dd() = link->sv().dot(link->hhv()) + link->sw().dot(link->hhw()) + link->Jm2();
                          // uu = u - hh^T*c + s^T*pp
                          link->uu() =
                              stdx::clamp(link->u(), link->u_lower(), link->u_upper()) -
                              (link->hhv().dot(link->cv()) + link->hhw().dot(link->cw()) +
                               link->sv().dot(link->pf()) + link->sw().dot(link->ptau()));
                      }
                  }
              }
          }

あたりを眺めると、慣性行列にrotorInertiaを1回足す方が順動力学計算結果と整合性が取れるように思うのですが、いかがでしょうか？

nakaoka · November 26, 2022, 11:28am

ご指摘ありがとうございます。
確かにご指摘の内容で間違いないように思います。コードに誤りがありまして申し訳ありません。

修正としては、以下の行を削除すればよさそうですね。

こちら対応してコミットしたいと思います。
ご協力ありがとうございました。

nakaoka · December 9, 2022, 9:31am

こちら修正しました。