2010年2月21日日曜日

Care-O-botのシミュレーション(アーム編)

気が向いたのでCare-O-botのアームのほうも動かします。

インストール
$ rosdep install cob3_gazebo cob3_arm
$ rosmake cob3_gazebo cob3_arm

動かす
$ roscd cob3_gazebo/ros/launch
$ roslaunch cob3_arm.launch


えー!
アームだけっすか。ビビった。

やはりJointSplineTrajectoryControllerを使って動かすみたいです。

$ rosrun cob3_arm simple_trajectory.py
とするとぐにゃぐにゃと動きます。

cob3_arm/ros/src/simple_trajectory.py
に軌道がかかれているので、これを変更すれば自由にアームを動かせますね。

次にGUIを使って操作する方法もあるのでこれを紹介します。

インストール
$ rosdep install cob3_dashboard
$ rosmake cob3_dashboard

実行
$ rosrun cob3_dashboard knoeppkes.py


ボタンを押すと動きますがあまり実用的ではなさそうです。
自分で軌道作って登録する感じでしょうか?

今日はここまで。

アームと台車が連動して動くかと思っていたのでちょっと残念ですね。
がんばればすぐできると思いますが。。。

2010年2月20日土曜日

Care-O-botのシミュレーションを試す

ドイツのCare-O-botのソフトがROSベースで公開されましたね。
私はこのロボットかなり好きだったので、ちょっとうれしいような、くやしいような。

さっそくかるーく試してみます。

まずはソフトのインストールから。

http://www.care-o-bot-research.org/software/care-o-bot-in-use

Gitを使ってDLするみたいなんで、git使っていない人はインストールしましょう。


$ sudo apt-get install git git-core gitk

で、次にgithubというサイトを使ってコードを配布しているので、
ここのアカウントがない人は作ります。

以下のサイトにいくと、sign upって書いてあると思うので、そこからsign upしてください。

http://github.com/ipa320/care-o-bot

※参考
そのときに、sshのpublicキーを要求されるので、

$ ssh-keygen -t rsa

として、
$HOME/.ssh/id_rsa.pub
の中身をコピペします。

gitは使ったことないので、不安ですが、
$ cd ~/ros/pkgs
$ git clone http://github.com/ipa320/care-o-bot.git care-o-bot

してコードをゲットしました。

$ cd ~/ros/pkgs/care-o-bot
$ . makeconfig
をターミナルで必ずするようにと書いてあります。

そして

$ rosdep install cob3_gazebo teleop_base
$ rosmake cob3_gazebo teleop_base
しましょう。

$ roscd cob3_gazebo/ros/launch
$ roslaunch cob3_pr2base.launch

シミュレータが上がりました。
ちょっと上から降ってくるので、びっくりしましたw
アームなしバージョンなんですね。

そして、以下のようにするとキーボードで動かせます。

$ rosrun teleop_base teleop_base_keyboard


今回はこれだけにしておきます。
アームもやりたい!

2010年2月17日水曜日

ROSチュートリアル in ICRA2010

ICRA2010という学会でROSのチュートリアルがあるみたいですね。

http://www.ros.org/wiki/Events/ICRA2010Tutorial

2日間にわたりがっつりです。
内容はこのブログと似た感じになりますね。

2010年2月13日土曜日

OpenRAVEをインストールする

プランニングのライブラリOpenRAVE
すごいですよね。
写真見るだけでもわくわくしますね。
http://openrave.programmingvision.com/index.php/Main_Page

これがROSとつながります。
http://www.ros.org/wiki/openrave
http://openrave.programmingvision.com/index.php/Robotics_Manipulation_System


今回はこれをインストールしましょう。

まずはsubversionでソースをもらいましょう。



svn co https://cmu-ros-pkg.svn.sourceforge.net/svnroot/cmu-ros-pkg/trunk ~/ros/pkgs/cmu-ros-pkg 

~/.octavercに以下を追加


more off
[status,rosoctpath] = system(['rospack find rosoct']);
rosoctpath = strtrim(rosoctpath);
addpath(fullfile(rosoctpath, 'octave'));
addpath(fullfile(rosoct_findpackage ('openrave'),'share','openrave','octave'))

$ rosdep install openraveros orrosplanning
で必要な外部ソフトをインストール。結構ありますね。

$ rosmake openraveros orrosplanning robot_openrave_control
でmakeします。

そして、~/ros/setup.shを以下のように編集します。
環境によって設定が変わると思うので参考程度に。
cp ~/ros/setup.sh ~/ros/setup.sh.org
してから編集しましょう。
最悪ROSが動かなくなります。

export ROS_ROOT=$HOME/ros/ros
export PATH=$ROS_ROOT/bin:$PATH
export PYTHONPATH=$ROS_ROOT/core/roslib/src:$PYTHONPATH
if [ ! "$ROS_MASTER_URI" ] ; then export ROS_MASTER_URI=http://localhost:11311 ; fi
export ROS_PACKAGE_PATH=/home/ogutti/ros/pkgs

export OCTAVE_PATH=$OCTAVE_PATH:$ROS_PACKAGE_PATH/ros_experimental/rosoct/octave
export ROS_PARALLEL_JOBS=-j1

export PATH=$PATH:`rospack find openrave`/bin
export PYTHONPATH=$PYTHONPATH:`rospack find openrave`/share/openrave
export OPENRAVE_DATA=$OPENRAVE_DATA:`rospack find visibilitygrasping`:`rospack find openrave`/share/openrave

source $ROS_ROOT/tools/rosbash/rosbash

現在のシェルを閉じて、
環境変数を新しくするために新しいシェルを立ち上げてください。
では試しましょう。



rosrun robot_openrave_control simulationserver --robotfile robots/schunk-lwa3.robot.xml --jointname j0 --jointname j1 --jointname j2 --jointname j3 --jointname j4 --jointname j5 --jointname j6
もしくは
roslaunch `rospack find robot_openrave_control`/schunkarm_sim.ros.xml
    を実行しましょう。

    これでコントローラーがあがりました。

    そして以下のようにするとなんかロボットがでます。

    roslaunch `rospack find robot_openrave_control`/wam_sim.ros.xml
    

    なんか、いろいろ動いています。

    今回は中身には触れませんが、動いたのでいろいろ試したいと思います。

    2010年2月12日金曜日

    i-SOBOT in Gazeboの進捗

    まだサイズとか軸方向とか適当ですけど、
    ロボットっぽくなってきましたねぇ。

    まだ、腕がふらふらします。


    サーボは初期化時に、
          joint->SetParam(dParamVel, 0);
          joint->SetParam(dParamFMax, JOINT_MAX_FORCE);



    として、サーボループで、


          hj->SetParam(dParamVel, effort_command);
    という感じでサーボしています。
    どうなんでしょう。

    ほとんどもうgazebo勉強記録になってきました。


    URDFをさらしておきます。

    <robot xmlns:xacro="http://ros.org/wiki/xacro"
           xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
           xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
           name="simple_box">

      <!-- *************** JOINT Definition *****************-->
      <xacro:macro name="my_joint" params="name parent child *axis *origin">
        <joint name="${name}" type="revolute" >
          <limit lower="-3" upper="3" effort="10.0" velocity="2.0" />
          <parent link="${parent}" />
          <child link="${child}" />
          <xacro:insert_block name="axis" />
          <xacro:insert_block name="origin" />
        </joint>
      </xacro:macro>

      <!-- *************** LINK Definition *****************-->
      <xacro:macro name="my_link" params="name mass color z *box">
        <link name="${name}">
          <inertial>
       <mass value="${mass}" />
    <!-- center of mass (com) is defined w.r.t. link local coordinate system  -->
       <origin xyz="0 0 ${z}" />

       <inertia  ixx="${0.001 * mass}" ixy="0.0"  ixz="0.0"  iyy="${0.001 * mass}"  iyz="0.0"  izz="${0.001 * mass}" />
          </inertial>
          <visual>
    <!-- visual origin is defined w.r.t. link local coordinate system -->
       <origin xyz="0 0 ${z}" rpy="0 0 0" />
       <geometry name="${name}_visual_geom">
              <xacro:insert_block name="box" />
       </geometry>
          </visual>
          <collision>
    <!-- collision origin is defined w.r.t. link local coordinate system  -->
       <origin xyz="0 0 ${z}" rpy="0 0 0 " />
       <geometry name="${name}_collision_geom">
              <xacro:insert_block name="box" />
       </geometry>
          </collision>
        </link>
        <gazebo reference="${name}">
          <material>Gazebo/${color}</material>
          <turnGravityOff>false</turnGravityOff>
        </gazebo>
      </xacro:macro>

      <!-- *************** LEG Definition *****************-->
      <xacro:macro name="my_leg" params="name y">
        <!-- joint definition -->

        <xacro:my_joint name="${name}_joint1" parent="torso_link1" child="${name}_link1">
          <axis xyz="1 0 0"/>
          <origin xyz="0 ${y * 0.05} 0.0" rpy="0 0 0" />
        </xacro:my_joint>
        <xacro:my_joint name="${name}_joint2" parent="${name}_link1" child="${name}_link2">
          <axis xyz="0 1 0"/>
          <origin xyz="0 0 -0.02" rpy="0 0 0" />
        </xacro:my_joint>
        <xacro:my_joint name="${name}_joint3" parent="${name}_link2" child="${name}_link3">
          <axis xyz="0 1 0"/>
          <origin xyz="0 0 -0.04" rpy="0 0 0" />
        </xacro:my_joint>
        <xacro:my_joint name="${name}_joint4" parent="${name}_link3" child="${name}_link4">
          <axis xyz="0 1 0"/>
          <origin xyz="0 0 -0.04" rpy="0 0 0" />
        </xacro:my_joint>
        <xacro:my_joint name="${name}_joint5" parent="${name}_link4" child="${name}_link5">
          <axis xyz="1 0 0"/>
          <origin xyz="0 0 -0.04" rpy="0 0 0" />
        </xacro:my_joint>
        
        <xacro:my_link name="${name}_link1" mass="0.1" z="0.0" color="Blue">
          <box size="0.07 0.03 0.04" />
       </xacro:my_link>
        <xacro:my_link name="${name}_link2" mass="0.1" z="-0.02" color="Red">
          <box size="0.07 0.06 0.04" />
        </xacro:my_link>
        <xacro:my_link name="${name}_link3" mass="0.1" z="-0.02" color="Green">
          <box size="0.07 0.06 0.04" />
        </xacro:my_link>
        <xacro:my_link name="${name}_link4" mass="0.2" z="-0.02" color="Blue">
          <box size="0.10 0.06 0.04" />
        </xacro:my_link>
        <xacro:my_link name="${name}_link5" mass="0.2" z="-0.01" color="White">
          <box size="0.2 0.08 0.02" />
        </xacro:my_link>
      </xacro:macro>
      

      <!-- *************** ARM Definition *****************-->
      <xacro:macro name="my_arm" params="name y">
        <!-- joint definition -->

        <xacro:my_joint name="${name}_joint1" parent="torso_link1" child="${name}_link1">
          <axis xyz="0 1 0"/>
          <origin xyz="0 ${y * 0.08} 0.15" rpy="0 0 0" />
        </xacro:my_joint>
        
        <xacro:my_joint name="${name}_joint2" parent="${name}_link1" child="${name}_link2">
          <axis xyz="1 0 0"/>
          <origin xyz="0 ${y * 0.03} 0.01" rpy="0 0 0" />
        </xacro:my_joint>

        <xacro:my_joint name="${name}_joint3" parent="${name}_link2" child="${name}_link3">
          <axis xyz="1 0 0"/>
          <origin xyz="0 0 -0.085" rpy="0 0 0" />
        </xacro:my_joint>

        <xacro:my_link name="${name}_link1" mass="0.1" z="0.0" color="Blue">
          <box size="0.05 0.030 0.02" />
       </xacro:my_link>
        <xacro:my_link name="${name}_link2" mass="0.1" z="-0.035" color="Red">
          <box size="0.05 0.03 0.07" />
        </xacro:my_link>
        <xacro:my_link name="${name}_link3" mass="0.1" z="-0.035" color="Green">
          <box size="0.05 0.03 0.10" />
        </xacro:my_link>
      </xacro:macro>

      <!-- *************** HEAD Definition *****************-->
      <xacro:macro name="my_head" params="name">
        <!-- joint definition -->

        <xacro:my_joint name="${name}_joint1" parent="torso_link1" child="${name}_link1">
          <axis xyz="0 0 1"/>
          <origin xyz="0 0 0.2" rpy="0 0 0" />
        </xacro:my_joint>

        <xacro:my_link name="${name}_link1" mass="0.1" z="0.0" color="Green">
          <box size="0.08 0.07 0.05" />
        </xacro:my_link>
      </xacro:macro>

      <!-- *************** TORSO Definition *****************-->
      <xacro:my_link name="torso_link1" mass="0.1" z="0.1" color="Blue">
        <box size="0.12 0.15 0.15" />
      </xacro:my_link>

      <xacro:my_leg name="my_rleg" y="1" />
      <xacro:my_leg name="my_lleg" y="-1" />

      <xacro:my_arm name="my_rarm" y="1" />
      <xacro:my_arm name="my_larm" y="-1" />

      <xacro:my_head name="my_head1" />

      <gazebo>
        <controller:gazebo_ros_test name="gazebo_ros_test" plugin="libgazebo_ros_test.so">
          <interface:audio name="gazebo_ros_test_dummy_interface" />
          <alwaysOn>true</alwaysOn>
          <updateRate>1.0</updateRate>
          <timeout>5</timeout>
        </controller:gazebo_ros_test>
      </gazebo>
    </robot>

    2010年2月11日木曜日

    PR2の台車が動かない?

    以前、PR2の台車を動かすチュートリアルを紹介しました。
    記事:http://ros-robot.blogspot.com/2010/01/pr2_06.html
    本家チュートリアル:http://www.ros.org/wiki/pr2/Tutorials/Using%20the%20robot%20base%20controllers%20to%20drive%20the%20robot

    これが現在動きません。

    この原因を探っていくと、
    ~/ros/pkgs/pr2_controllers/pr2_mechanism_controllers/src/
    にある
    pr2_base_controller.cppの、


      //  cmd_sub_deprecated_ = root_handle_.subscribe<geometry_msgs::Twist>("cmd_vel", 1, &Pr2BaseController::commandCallback, this);
      cmd_sub_ = node_.subscribe<geometry_msgs::Twist>("command", 1, &Pr2BaseController::commandCallback, this);

    という記述があります。
    以前は
    cmd_vel
    というトピックで動かされていたのですが、これがdeprecatedになり、
    現在は
    command
    というトピックに変更になったようです。

    また、ネームスペースも切られていて、
    $ rostopic list
    とすると
    /base_controller/command
    というのがあります。

    試しに動かすために、
    $ rostopic pub /base_controller/command geometry_msgs/Twist '[0, 0, 0]' '[0, 0, 0]' 

    とすると、ロボットが動きました。

    ということで、このチュートリアルをどう変更すればいいか分かりますか?

    私は以下のようにしました。

    #include <iostream>

    #include <ros/ros.h>
    #include <geometry_msgs/Twist.h>

    class RobotDriver
    {
    private:
      //! The node handle we'll be using
      ros::NodeHandle nh_;
      //! We will be publishing to the "cmd_vel" topic to issue commands
      ros::Publisher cmd_vel_pub_;

    public:
      //! ROS node initialization
      RobotDriver(ros::NodeHandle &nh)
      {
        nh_ = nh;
        //set up the publisher for the cmd_vel topic
        cmd_vel_pub_ = nh_.advertise<geometry_msgs::Twist>("command", 1);
      }

      //! Loop forever while sending drive commands based on keyboard input
      bool driveKeyboard()
      {
        std::cout << "Type a command and then press enter.  "
          "Use '+' to move forward, 'l' to turn left, "
          "'r' to turn right, '.' to exit.\n";

        //we will be sending commands of type "twist"
        geometry_msgs::Twist base_cmd;

        char cmd[50];
        while(nh_.ok()){

          std::cin.getline(cmd, 50);
          if(cmd[0]!='+' && cmd[0]!='l' && cmd[0]!='r' && cmd[0]!='.')
          {
            std::cout << "unknown command:" << cmd << "\n";
            continue;
          }

          base_cmd.linear.x = base_cmd.linear.y = base_cmd.angular.z = 0;   
          //move forward
          if(cmd[0]=='+'){
            base_cmd.linear.x = 0.25;
          } 
          //turn left (yaw) and drive forward at the same time
          else if(cmd[0]=='l'){
            base_cmd.angular.z = 0.75;
            base_cmd.linear.x = 0.25;
          } 
          //turn right (yaw) and drive forward at the same time
          else if(cmd[0]=='r'){
            base_cmd.angular.z = -0.75;
            base_cmd.linear.x = 0.25;
          } 
          //quit
          else if(cmd[0]=='.'){
            break;
          }
          
          //publish the assembled command
          cmd_vel_pub_.publish(base_cmd);
        }
        return true;
      }

    };

    int main(int argc, char** argv)
    {
      //init the ROS node
      ros::init(argc, argv, "robot_driver");
      ros::NodeHandle nh = ros::NodeHandle("base_controller");

      RobotDriver driver(nh);
      driver.driveKeyboard();
    }


    赤いところが変更したところです。

    ROSの開発にドキュメントがついていけてない感じですね。。。

    2010年2月10日水曜日

    オリジナルロボット進捗その1

    全然進んでいないのですが、
    今やっとgazeboでサーボがまともに動くようになりました。


    二足ロボットがやっと立てました。

    シミュレーションパラメータも二足用になっていないので、チューンしないといけないかもしれません。

    2010年2月6日土曜日

    シミュレータの中のオリジナルロボットを動かすための準備その2

    前回振り子(名前だけで実際は□が回転するだけ)のシミュレーションをして、
    test_controllerという実機互換?のコントローラーで動かしました。

    しかし肝心のロボットモデルとコントローラーをつなぐgazebo_pluginが出てきませんでした。
    今回その中身を見ていきます。

    $ roscd pr2_examples_gazebo/single_link_defs
    $ ls
    しましょう。

    single_link.transmission.xacro  single_link.urdf.xacro  single_link_position_controller.yaml

    と3つのファイルがあります。
    前回出てきたのはこのうち2つで、
    single_link.urdf.xacro ・・・・モデルファイル
    single_link_position_controller.yaml・・・・なんらかのパラメータ

    ということろまでやりました。

    ではモデルファイルから見ていきます。

    全部張るのはちょっと冗長なので省略しますので、lessなので見ながら読んでください。
    最初は<robot>ですが、xmlns:xiなど、xmlnsで始まる記述がつづきます。
    これはXMLの名前空間で、gazeboのジョイントなどの定義のために必要です。
    すべてが必要という訳ではありませんが、いくつかは必須のはずです。

    <joint>, <link>, <gazebo>などは以前見たものと完全に同じですね。
    最後の<gazebo>の中の、
          <controller:gazebo_ros_time name="gazebo_ros_time" plugin="libgazebo_ros_time.so">
            <alwaysOn>true</alwaysOn>
            <updateRate>100.0</updateRate>
            <interface:audio name="dummy_gazebo_ros_time_iface_should_not_be_here"/>
          </controller:gazebo_ros_time>

    からはあたらいいですね。

    この部分がシミュレータの中のロボットを直接動かす部分、gazebo_pluginの設定をしているところです。

    これで、libgazebo_ros_time.soというバイナリで定義されるgazebo_ros_timeというgazebo_pluginをこのモデルに結びつけています。
    この中の<alwaysOn>などはオプション指定のようなものだと思います。
    <interface:audio>の部分はまったく分かりませんが、これを指定しないと動きません。
    このgazebo_ros_timeというプラグインは/clockというトピックに、シミュレータ時間をpublishします。

    ↓参照

    次が一番重要なところです。

          <!-- PR2_ACTARRAY -->
          <controller:gazebo_ros_controller_manager name="gazebo_ros_controller_manager" plugin="libgazebo_ros_controller_manager.so">
            <alwaysOn>true</alwaysOn>
            <updateRate>100.0</updateRate>
            <interface:audio name="gazebo_ros_controller_manager_dummy_iface" />
          </controller:gazebo_ros_controller_manager>
    もう分かると思いますがlibgazebo_ros_controller_manager.soというバイナリからgazebo_ros_controller_managerというプラグインを呼び出し、シミュレータに結びつけています。

    このプラグインは
    pr2_gazebo_pluginパッケージの中にあります。

    中を見るとこれが、PR2実機におけるpr2_controller_managerの代わりをシミュレータ上でやるプログラムであることが分かります。

    なので、
    gazebo_ros_controller_managerhはpr2_controller_managerの代わりに
    test_controllerなどの実機互換のコントローラーを回しつつ、その出力をシミュレータに反映するという構成であったことが分かります。

    なので、前回書いた
    gazebo <-> gazebo_plugin <-> controller
    という図はある意味正しいですが、
    そのcontrollerを駆動しているのがgazebo_plugin(gazebo_ros_controller_manager)という複雑な構成でした。

    図で書くと以下のようになっていると思います。



    そしてよく見るとsingle_link.urdf.xacroの最後に

      <include filename="./single_link.transmission.xacro" />
    があります。
    これはpr2_controller_managerが使う設定ファイルのようです。


    ということで、pr2_controller_managerを使う実機プログラムは今回のように、
    gazebo_ros_controller_managerを使えばいいことが分かりました。

    ROSではpr2_controller_managerを使ってロボットを動かすことが前提となっているようです。
    たしかに実時間制御をするにはそれが一番楽なのでしょう。
    なので、そのうちpr2_controller_managerまわりの勉強をするべきです。
    このブログでは不幸にもそこは飛ばしてしまいました。
    手元の環境では実時間制御どうせできないので不要と考えたから意図的に飛ばしました。

    まともなロボットは実時間制御を使うので、PR2に限らずこのようなやり方(pr2_controller_manager(実機)とgazebo_ros_controller_managerプラグイン(シミュレータを切り分けるやり方)をするのがいいでしょう。


    しかし、私は目標として、改造i-SOBOTをシミュレータに入れることを想定していたので、
    実時間制御は使いません。
    そのため、このやり方ではなく自分でプラグインを書く方法を試しました。

    まあ、仕組みがここまで分かれば簡単です。
    .soを作成して、urdfファイルでそれを指定すればいいだけですので。

    そのうち紹介します。

    自律移動ソフトを試す

    だんだんやることがハードになってきて、気軽にblogかけなくなってきました。

    現実逃避に自律移動ソフトで遊びました。

    http://www.ros.org/wiki/pr2_simulator/Tutorials/2DNavigationStackDemoWithSimple2DesksWorld
    http://www.ros.org/wiki/navigation/Tutorials/RobotSetup

    まずはmakeです。

    $ rosdep install pr2_2dnav_gazebo rviz
    $ rosmake pr2_2dnav_gazebo rviz
    


    結構時間かかります。

    で、以下のようにしてまずシミュレータを上げます。

    $ roscd pr2_2dnav_gazebo
    $ export ROBOT=sim
    $ roslaunch pr2-simple-fake_localization.launch

    こんな感じの環境ができます。

    次にロボットに指令を出すためにrvizというソフトを立ち上げます。

    $ roslaunch 2dnav_pr2 rviz_move_base.launch

    こんな感じで、
    • マップ(黒いところが壁)
    • ロボットの推定自己位置(ロボットがいます)
    • ロボットの自己位置候補(赤矢印)
    • レーザーレンジファインダでの観測点(緑の点)
    • 3Dスキャナによるポイントクラウド(赤や青)
    が表示されます。

    メニューで、2D Nav Goalというボタンを押してから、
    適当な場所をドラッグ(ドラッグ開始点が目標地点、開始点と終了点を結ぶ方向が目標方向)することでロボットに目標位置姿勢を与えることができます。

    赤い矢印が指定した目標位置姿勢になります。


    するとシミュレータ上のロボットも動きます。

    これはfake_localizationという名前から分かるように、自己位置はシミュレータから直接取得しているため、
    ロボットの自己位置がシミュレーションとずれることがありません。

    ちゃんと自己位置を推定させるには以下のようにします。
    (一旦、gazeboもrvizも終了させてください)



    $ roslaunch pr2-armless-simple-amcl.launch


    $ roslaunch 2dnav_pr2 rviz_move_base.launch

    するとシミュレータはさきほどと同じですが、
    rviz上(ロボットの認識結果)はいきなりロボットがすごくずれた位置(0,0,0)?にいます。

    2D Pose Estimateのボタンを押してから、さきほど目標位置姿勢を与えたように、
    ロボットの自己位置をいれてあげましょう。
    緑のレーザーが地図とずれているのでまだ自己位置がずれていますね。
    これくらいは修正してほしいところです。
    ではこのまま、2D Nav Goalを使ってロボットを動かしてみましょう。
    なかなか収束しませんが、特徴量が少ない空間なので仕方ないかもしれませんね。


    自己位置推定プログラムの良し悪しを測れそうですね。

    今回はライトな内容でした。
    そのうち、自律移動ソフトの中身について見ていきたいと思います。

      シミュレータの中のオリジナルロボットを動かすための準備その1

      シミュレータの中のオリジナルロボットを動かしたいのですが、
      そのために参考になるチュートリアルがあるので紹介します。

      振り子シミュレーション

      シミュレータ内に振り子を作って、それを制御します。
      主にgazeboを考慮したモデルの作り方について学びます。

      さっそくですが、
      $ roslaunch gazebo empty_world.launch
      でgazeboを走らせて、

      $ roslaunch pr2_examples_gazebo single_link.launch
      しましょう。

      こんなんがでます。

      $ rosrun pr2_controller_manager pr2_controller_manager list
      すると、
      test_controller ( running )

      とでます。
      $ rostopic pub /test_controller/command std_msgs/Float64 .5
      ぶるんぶるん、と少し回転します。

      では中身を見ていきましょう。

      まずはlaunchファイルから。

      <launch>
        <!-- send single_link.urdf to param server -->
        <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_examples_gazebo)/single_link_defs/single_link.urdf.xacro'" />

        <!-- push robot_description to factory and spawn robot in gazebo -->
        <node name="spawn_single_link" pkg="gazebo_tools" type="gazebo_model" args="-p robot_description spawn" respawn="false" output="screen" />

        <rosparam file="$(find pr2_examples_gazebo)/single_link_defs/single_link_position_controller.yaml" command="load"/>

        <node name="spawn_test_controller" pkg="pr2_controller_manager" type="spawner" args="test_controller" respawn="false" output="screen" />

      </launch>

      まずは最初の<param>から。
      これはroscoreが持つパラメータサーバに、single_link.urdf.xacroをxacroを使って展開した結果を送信しています。パラメータの名前としてrobot_descriptionを使っています。
      追々分かりますが、ROSでは基本的にロボットモデルはrobot_descriptionという名前のパラメータとしてサーバにアップロードするもののようです。
      (PR2だけなのかROSのルール(慣習)なのか微妙なのですが)

      次の<node>でgazebo(シミュレータ)にモデルを生成させています。
      $ rosrun gazebo_tools gazebo_model -p robot_description spawn
      と中身は一緒です。

      $ rosrun  gazebo_tools gazebo_model --help
      すると詳細が分かりますが、
      robot_descriptionという名前のパラメータを使ってgazeboにモデルをspawnするという意味です。

      次の<rosparam>はそのままで、rosparamコマンドでパラメータを読み込んでいます。
      $ rosparam load `rospack find pr2_examples_gazebo`/single_link_defs/single_link_position_controller.yaml
      と同じですね。

      最後の<node>がシミュレータ上のロボットや、実機を動かすための制御装置(コントローラー)です。

      $ rosrun pr2_controller_manager spawner test_controller :name=spawn_test_controller
      と同じですね。
      pr2_controller_managerを使って、test_controllerというリアルタイム制御装置を駆動します。

      gazeboでは、

      gazebo <-> gazebo_plugin <-> test_controller

      という構成でシミュレータ上のモデルを動かすはずです。
      今回はgazebo_pluginが出てきませんでした。

      それがどこに隠されているか、次回に続きます。

      2010年2月5日金曜日

      WGのpr2とrobotの考え方の推測

      ROSについて少し誤解をしていました。

      今までPR2というのはWGのロボットの一つ、だと思っていて、
      「pr2_*というプログラムは、当然robot_*というプログラムを継承して作ってある、PR2用のプログラムである」
      と思っていました。

      事実、
      pr2_mechanism_controllersに対して、robot_mechanism_controllersが存在します。
      なので、当然pr2_mechanism_controllersの中身はrobot_mechanism_controllersの何らかのクラスを継承しているものだと勘違いしていました。

      そして、pr2_*が78ものパッケージが存在するのに対し、robot_*が11しか存在しないことに疑問を持ち、
      「WGはロボットの仮想化に興味がないのか?PR2意外のロボットはやる気がない?ROSっていろんなロボットを動かすためにあるんじゃないの?なんなの?」
      と常々疑問に思っていました。

      しかし、最近シミュレータコントローラーを書くためにいろいろ格闘した結果、ひとつの結論に達しました。

      「ROSにおいてpr2は一般的に言う所のrobotを意味する」

      なので、pr2_*というパッケージ名にだまされてはいけません。

      pr2_controller_managerは一般的な思想でいうところのrobot_controller_managerを意味するのです。
      なので、pr2_*で始まるパッケージを自信を持って自分のロボットに使えばいいです。
      もちろんそういうふうに使えるように書かれています。

      (”OpenHRPでいうところのHumanoid”=”ROSでいうところのPR2”
      とでも考えればいいと思います。ってマニアックすぎて意味分からないですね。)


      ということで次回堂々とpr2_*なプログラムを自分ロボットに使います。

      全然勘違いしてたらすみません。つっこみ歓迎です。

      2010年2月3日水曜日

      xacroを使う

      諸事情により環境がUbuntu9.10になりました。
      ROS1.0は普通に動いていますね。

      今回はシミュレータのmakeがまだ終わらないので寄り道です。

      xacroはXML macroの略です。
      XMLをいちいち手書きするのは大変なのでマクロを使って楽しようという
      画期的なソフトです。

      urdf形式でロボットモデルを作るときに、
      右手と左手をいちいち定義するよりも、腕マクロを作り、そこに引数として「右」、「左」を入れたほうが楽ですよね。

      そんな感じで使っているみたいです。
      まさにその例が以下です。








      <xacro:macro name="pr2_arm" params="suffix parent reflect">
        <pr2_upperarm suffix="${suffix}" reflect="${reflect}" parent="${parent}" />
        <pr2_forearm suffix="${suffix}" reflect="${reflect}" parent="elbow_flex_${suffix}" />
      </xacro:macro>
      
      <xacro:pr2_arm suffix="left" reflect="1" parent="torso" />
      <xacro:pr2_arm suffix="right" reflect="-1" parent="torso" />

      これが↓のように展開されます。

      <pr2_upperarm suffix="left" reflect="1" parent="torso" />
      <pr2_forearm suffix="left" reflect="1" parent="elbow_flex_left" />
      <pr2_upperarm suffix="right" reflect="-1" parent="torso" />
      <pr2_forearm suffix="right" reflect="-1" parent="elbow_flex_right" />
      この例ではむしろ書くコード量が増えていますが、不要なコピペはしなくて済みますね。


      パラメータ

      <xacro:parameter name="the_radius" value="2.1" />
      <xacro:parameter name="the_length" value="4.5" />
      
      <geometry type="cylinder" radius="${the_radius}" length="${the_length}" />
      上記のようにしてparameterをセットすることができます。
      要するに
      the_radius = 2.1
      the_length = 4.5
      と代入し、${the_radius}, ${the_length}で参照することができます。
      このパラメータ節はXML中の好きな場所に置くことができます。

      もうひとつのパラメータがパラメータブロックで、以下のようにして使います。
      <xacro:parameter name="front_left_origin">
        <origin xyz="0.3 0 0" rpy="0 0 0" />
      </xacro:parameter>
      
      <pr2_wheel name="front_left_wheel">
        <xacro:insert_block name="front_left_origin" />
      </pr2_wheel>
      パラメータブロックは<xacro:insert_block>を使って挿入します。

      計算
      先ほど書いた${}の中では算術演算することができます。
      簡単な計算(四則演算?)が使えるみたいです。


      <xacro:parameter name="pi" value="3.1415926535897931" />
      
      <circle circumference="${2.5 * pi}" />



      Rospackコマンド
      ${}の中ではrospackコマンドと同じことができます。
      ここにあるオプションが使えます。

      <foo value="$(find xacro)" />

      マクロ

      xacroのメイン昨日です。パラメータとマクロのタグを使ってマクロを定義します。
      パラメータはスペースなどで区切ります。

      以下に例を示します。

      <xacro:macro name="pr2_caster" params="suffix *origin">
        <joint name="caster_${suffix}_joint">
          <axis xyz="0 0 1" />
        </joint>
        <link name="caster_${suffix}">
          <xacro:insert_block name="origin" />
        </link>
      </xacro:macro>
      
      <xacro:pr2_caster suffix="front_left">
        <pose xyz="0 1 0" rpy="0 0 0" />
      </xacro:pr2_caster>
      suffixとoriginという2つのパラメータを受け取っています。
      Cでいうと
      #define PR2_CASTER(suffix, origin) ...
      という感じでしょうか。

      ただし、originのほうにはスターがついていて、*originになっています。
      これはoriginが単なる変数ではなくパラメータブロックであることを示します。
      先ほどfront_left_originと言う名前ででてきました。


      pr2_casterを実際に使っているところを見ると、suffixについてはfront_leftと直接指定しています。一方で二つめの引数であるはずのoriginは定義していません。
      originは<xacro:pr2_caster>の中に含まれるすべてがoriginとしてマクロに渡されます。
      今回は<pose .. >の部分になりますね。


      なので展開結果は以下になります。







      <joint name="caster_front_left_joint">
        <axis xyz="0 0 1" />
      </joint>
      <link name="caster_front_left">
        <pose xyz="0 1 0" rpy="0 0 0" />
      </link>



      また、マクロは入れ子にすることができて、外側から順に展開されます。
      以下のように定義すると、







      <a>
        <xacro:macro name="foo" params="x">
          <in_foo the_x="${x}" />
        </xacro:macro>
      
        <xacro:macro name="bar" params="y">
          <in_bar>
            <xacro:foo x="${y}" />
          </in_bar>
        </xacro:macro>
      
        <xacro:bar y="12" />
      </a>

      以下のように展開されます。

      <a>
        <in_bar>
          <in_foo the_x="12.0"/>
        </in_bar>
      </a>



      また、xacroコマンドは以下のようにして使います。

      $ rosrun xacro xacro.py hoge.xacro
      とすると展開されたものがプリントされます。
      -oをつけるとファイルに出力できます。


      また、実際に使うときは、xmlns:xacroを指定しないといけません。
      では前回作ったurdfをxacroを使って書き直して見ます。



      <robot xmlns:xacro="http://ros.org/wiki/xacro"
             name="simple_box">
        <xacro:macro name="my_joint" params="name parent child z">
          <joint name="${name}" type="revolute" >
            <limit lower="-1" upper="1" effort="1" velocity="1" />
            <axis xyz="0 1 0"/>
            <parent link="${parent}" />
            <child link="${child}" />
            <origin xyz="0 0 ${z}" rpy="0 0 0" />
          </joint>
        </xacro:macro>

        <xacro:macro name="my_link" params="name mass color z *box">
          <link name="${name}">
            <inertial>
              <mass value="${mass}" />
              <!-- center of mass (com) is defined w.r.t. link local coordinate system  -->
              <origin xyz="0 0 ${z}" />

              <inertia  ixx="${mass}" ixy="0.0"  ixz="0.0"  iyy="${mass}"  iyz="0.0"  izz="${mass}" />
            </inertial>
            <visual>
              <!-- visual origin is defined w.r.t. link local coordinate system -->
              <origin xyz="0 0 ${z}" rpy="0 0 0" />
              <geometry name="${name}_visual_geom">
                <xacro:insert_block name="box" />
              </geometry>
            </visual>
            <collision>
              <!-- collision origin is defined w.r.t. link local coordinate system  -->
              <origin xyz="0 0 ${z}" rpy="0 0 0 " />
              <geometry name="${name}_collision_geom">
                <xacro:insert_block name="box" />
              </geometry>
            </collision>
          </link>
          <gazebo reference="${name}">
            <material>Gazebo/${color}</material>
            <turnGravityOff>false</turnGravityOff>
          </gazebo>
        </xacro:macro>

        <xacro:my_joint name="my_box_joint1" parent="my_box" child="my_box2" z="0.06"/>
        <xacro:my_joint name="my_box_joint2" parent="my_box2" child="my_box3" z="0.12"/>
        <xacro:my_link name="my_box" mass="3.0" z="0.0" color="Blue">
          <box size="0.1 0.10 0.10" />
        </xacro:my_link>
        <xacro:my_link name="my_box2" mass="1.0" z="0.05" color="Red">
          <box size="0.05 0.05 0.10" />
        </xacro:my_link>
        <xacro:my_link name="my_box3" mass="1.0" z="0.05" color="Green">
          <box size="0.05 0.05 0.10" />
        </xacro:my_link>
      </robot>



      となります。

      $ rosrun xacro xacro.py object.urdf.xacro > test.urdf
      などとしてurdfを出力できます。



      $ wc object.urdf.xacro
        54  185 1952 object.urdf.xacro
      $ wc object.urdf
       103  355 3281 object.urdf





      となり、行数的には半分になりました。
      編集もしやすいですね。

      これは画期的!と思ったんですがどうでしょうか?

      2010年2月1日月曜日

      シミュレータの中にオブジェクトを生成する



      今回はシミュレータに自分で定義した物体を入れます。


      とりあえずgazeboを起動しましょう。
      念のためmakeから。

      $ rosmake gazebo
      $ roslaunch gazebo empty_world.launch

      で、次に以下を適当な場所にobject.urdf
      として保存します。

      <robot name="simple_box">
        <joint name="my_box_joint" type="revolute" >
          <!-- axis is in the parent link frame coordintates -->
          <axis xyz="0 1 0" />
          <parent link="world" />
          <child link="my_box" />
          <!-- initial pose of my_box joint/link in the parent frame coordiantes -->
          <origin xyz="0 0 2" rpy="0 0 0" />
        </joint>
        <link name="my_box">
          <inertial>
            <mass value="1.0" />
            <!-- center of mass (com) is defined w.r.t. link local coordinate system -->
            <origin xyz="1 0 0" /> 
      
            <inertia  ixx="1.0" ixy="0.0"  ixz="0.0"  iyy="100.0"  iyz="0.0"  izz="1.0" />
          </inertial>
          <visual>
            <!-- visual origin is defined w.r.t. link local coordinate system -->
            <origin xyz="1 0 0" rpy="0 0 0" />
            <geometry name="my_box_visual_geom">
              <box size="0.05 0.05 0.10" />
            </geometry>
          </visual>
          <collision>
            <!-- collision origin is defined w.r.t. link local coordinate system -->
            <origin xyz="1 0 0" rpy="0 0 0 " />
            <geometry name="my_box_collision_geom">
              <box size="0.05 0.05 0.10" />
            </geometry>
          </collision>
        </link>
        <gazebo reference="my_box">
          <material>Gazebo/Blue</material>
          <turnGravityOff>false</turnGravityOff>
        </gazebo>
      </robot>

      中身は、
      1. <joint>ジョイントの定義(ワールド座標と次に定義するリンクとの仮想フリージョイント)
      2. <link>リンクの定義
        1. <inertial>剛体定義
        2. <visual>見え方定義
        3. <collision>衝突定義
      3. <gazebo>シミュレータ設定
      となっています。

      ではこれをシミュレータの中に生成してみます。
      $ rosrun gazebo_tools gazebo_model -f `pwd`/object.urdf -z 1 spawn my_object
      -zはモデルの初期Z座標です。

      [ERROR] 1265003610.139332000: Joint 'my_box_joint' is of type REVOLUTE but it does not specify limits
      [ERROR] 1265003610.139507000: joint xml is not initialized correctly
      [ERROR] 1265003610.139564000: Unable to load robot model from param server robot_description
      私の環境では以上のようにエラーがでてしまいました。
      仮想関節のタイプをrevoluteにするときは、限界角度を指定しないといけないみたいです。

      そこで、
      $ roscd gazebo_worlds
      $ less objects/mug-test.urdf
      を見てみると、
        <joint name="my_mug_joint" type="floating" >
      となっていますので、
      typeをfloatingにしてもう一度ためしてみます。


      今度は成功しました。
      ただし、
      [ WARN] 1265003926.244514000: URDF root link "world" is deprecated, please remove joint to world link from your URDF
      という表示がでました。
      たしかに、初期位置は最初のジョイントのoriginをいじっても変更できません。
      なので、苦労したfloatingジョイントですが削除しちゃいましょう。

      では関節を増やしてみましょう。


      <robot name="simple_box">

        <joint name="my_box_joint1" type="revolute" >
          <limit lower="-1" upper="1" effort="1" velocity="1" />
          <axis xyz="0 1 0"/>
          <parent link="my_box" />
          <child link="my_box2" />
          <origin xyz="0 0 0.06" rpy="0 0 0" />
        </joint>

        <joint name="my_box_joint2" type="revolute" >
          <limit lower="-1" upper="1" effort="1" velocity="1" />
          <axis xyz="0 1 0"/>
          <parent link="my_box2" />
          <child link="my_box3" />
          <origin xyz="0 0 0.12" rpy="0 0 0" />
        </joint>

        <link name="my_box">
          <inertial>
            <mass value="3.0" />
            <!-- center of mass (com) is defined w.r.t. link local coordinate system -->
            <origin xyz="0 0 0" />

            <inertia  ixx="3.0" ixy="0.0"  ixz="0.0"  iyy="3.0"  iyz="0.0"  izz="3.0" />
          </inertial>
          <visual>
            <!-- visual origin is defined w.r.t. link local coordinate system -->
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry name="my_box_visual_geom">
              <box size="0.1 0.10 0.10" />
            </geometry>
          </visual>
          <collision>
            <!-- collision origin is defined w.r.t. link local coordinate system -->
            <origin xyz="0 0 0" rpy="0 0 0 " />
            <geometry name="my_box_collision_geom">
              <box size="0.1 0.10 0.10" />
            </geometry>
          </collision>
        </link>

        <link name="my_box2">
          <inertial>
            <mass value="1.0" />
            <!-- center of mass (com) is defined w.r.t. link local coordinate system -->
            <origin xyz="0 0 0.05" />

            <inertia  ixx="1.0" ixy="0.0"  ixz="0.0"  iyy="100.0"  iyz="0.0"  izz="1.0" />
          </inertial>
          <visual>
            <!-- visual origin is defined w.r.t. link local coordinate system -->
            <origin xyz="0 0 0.05" rpy="0 0 0" />
            <geometry name="my_box_visual_geom2">
              <box size="0.05 0.05 0.10" />
            </geometry>
          </visual>
          <collision>
            <!-- collision origin is defined w.r.t. link local coordinate system -->
            <origin xyz="0 0 0.05" rpy="0 0 0 " />
            <geometry name="my_box_collision_geom2">
              <box size="0.05 0.05 0.10" />
            </geometry>
          </collision>
        </link>

        <link name="my_box3">
          <inertial>
            <mass value="1.0" />
            <!-- center of mass (com) is defined w.r.t. link local coordinate system -->
            <origin xyz="0 0 0.05" />

            <inertia  ixx="1.0" ixy="0.0"  ixz="0.0"  iyy="100.0"  iyz="0.0"  izz="1.0" />
          </inertial>
          <visual>
            <!-- visual origin is defined w.r.t. link local coordinate system -->
            <origin xyz="0 0 0.05" rpy="0 0 0" />
            <geometry name="my_box_visual_geom2">
              <box size="0.05 0.05 0.10" />
            </geometry>
          </visual>
          <collision>
            <!-- collision origin is defined w.r.t. link local coordinate system -->
            <origin xyz="0 0 0.05" rpy="0 0 0 " />
            <geometry name="my_box_collision_geom2">
              <box size="0.05 0.05 0.10" />
            </geometry>
          </collision>
        </link>


        <gazebo reference="my_box">
          <material>Gazebo/Blue</material>
          <turnGravityOff>false</turnGravityOff>
        </gazebo>
        <gazebo reference="my_box2">
          <material>Gazebo/Red</material>
          <turnGravityOff>false</turnGravityOff>
        </gazebo>
        <gazebo reference="my_box3">
          <material>Gazebo/Green</material>
          <turnGravityOff>false</turnGravityOff>
        </gazebo>
      </robot>


      Gazeboシミュレータのメニューで
      View -> Show Joints
      にチェックをいれておくと、ジョイント構造がみれてデバッグしやすいです。
      フリージョイントなんで、ふにゃー、っていう感じで倒れていきます。
      ちょっとイナーシャの設定があやしいですが、今回はこれくらいにして、
      次回コントローラーを作りましょう。