ROSでロボットの重心を推定する

ロボットの要素の中で大きなウエイトを占めるものに制御系があると思います。
制御系を設計するにあたり大切なものの一つに重心推定があると思います。
今回ロボットの制御系を勉強したいと思い、まずはROSでロボットの重心を推定するパッケージを自作してみました。
ソースコードはこちら
https://github.com/hakuturu583/robot_kinetics_pkgs
で公開しています。
基本的には前回の記事でまとめたコードをベースに開発しました。
このパッケージの動作する仕組み以下のようになっています。

cog_publisher_node.cpp

//headers in this package
# include <cog_publisher.h>

//headers for ROS
# include <ros/ros.h>

int main(int argc, char *argv[])
{
  // ROSノードとして初期化
  ros::init(argc, argv, "com_publisher");
  // cog_publisherクラスのコンストラクタ内で/robot_descriptionパラメータを取得、各リンクの重心、質量を入手
  cog_publisher center_of_gravity_publisher = cog_publisher();
  // /publish_rateパラメータを取得、重心位置のpublishレートを決定
  ros::Rate rate(center_of_gravity_publisher.get_publish_rate());
  // rosが機能している間以下の処理を繰り返す
  while(ros::ok())
  {
    // /tfトピックより/base_linkフレームから見た各リンクの重心位置を計算
    // 各リンクの重心位置と質量を合成してロボット全体の重心を推定
    // /cog/linksトピックで各リンクの重心をsensor_msgs/PointCloud形式でpublish
    // /cog/robotトピックでロボット全体の重心をgeometry_msgs/PointStamped形式でpublish
    center_of_gravity_publisher.publish();
    // /publish_rateパラメータに合わせてデータを出力するようにsleep
    rate.sleep();
  }
  return 0;
}

動作テスト

ピンク色の点が重心を表しています。
今回は実機ではなくgazeboでシミュレーションを行ってみました。
まずは初期姿勢から

左腕を開いて・・

上に上げて・・

最後に後ろへ

直感的にこんな動きするだろうなという動きしてますし、naoの実機で重心を取得（ALMotionProxy::getCOM）した時と似たような動きしてるので多分あってると思います・・・