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

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

http://www.ros.org/wiki/simulator_gazebo/Tutorials/SpawningObjectInSimulation

とりあえず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

にチェックをいれておくと、ジョイント構造がみれてデバッグしやすいです。

フリージョイントなんで、ふにゃー、っていう感じで倒れていきます。

ちょっとイナーシャの設定があやしいですが、今回はこれくらいにして、
次回コントローラーを作りましょう。