LoginSignup
1
1

More than 3 years have passed since last update.

@yaseinod

moveitでsbplプランナーが使えるまで

Last updated at Posted at 2019-08-08

OMPLとかCHOMPとかのプランナーでmoveitを使うチュートリアルがあるのにsbplプランナーを使うチュートリアルが存在しない！！のでせめて環境を作るところまでは書いておきたい。最終的にはpandaが動くようにしたい。また、moveitに自作のモーションプランナーを追加したい

環境

  • ubuntu16.04
  • kinetic
  • moveitはソースでコンパイル済

コケるポイント

  • 英語でも文献がほとんど存在しない
  • 他のプランナーはmoveitをsourceでコンパイルすれば済むがsbplはソースをcloneしないといけない
  • READMEで肝心なrunnigがcomming soon (10 month ago)

コケるまでの道のり

sbpl公式を参考に進めることにした。

cd ~/catkin_ws/src
git clone https://github.com/smpl/smpl
git clone https://github.com/sbpl/sbpl
cd sbpl && mkdir build && cd build && cmake .. && make && sudo make install
cd ~/catkin_ws/src
git clone https://github.com/aurone/leatherman
catkin build

すると以下のような問題に直面した。

問題点

rosdep installした時に怒られる

ワークスペースのsrcディレクトリにsmplをクローンしてleathermanをクローンしてrosdep installした時に怒られる。
調べたところleathermanパッケージを見つけることができていないことが理由らしいが、私は同じディレクトリに配置している。何故だ・・・
これを無視した場合多分依存関係を解決できていないのでひとつづつ入れないといけない(多分)

$ rosdep install --from-paths smpl -i -y
ERROR: the following packages/stacks could not have their rosdep keys resolved
to system dependencies:
sbpl_collision_checking: Cannot locate rosdep definition for [leatherman]
sbpl_collision_checking_test: Cannot locate rosdep definition for [leatherman]
smpl_ros: Cannot locate rosdep definition for [leatherman]
sbpl_pr2_robot_model: Cannot locate rosdep definition for [leatherman]
sbpl_kdl_robot_model: Cannot locate rosdep definition for [leatherman]
smpl_test: Cannot locate rosdep definition for [leatherman]

octomapが見つからないと怒られれる

CMake Error at /opt/ros/kinetic/share/catkin/cmake/catkinConfig.cmake:83 (find_package):
  Could not find a package configuration file provided by "octomap_ros" with
  any of the following names:

    octomap_rosConfig.cmake
    octomap_ros-config.cmake

  Add the installation prefix of "octomap_ros" to CMAKE_PREFIX_PATH or set
  "octomap_ros_DIR" to a directory containing one of the above files.  If
  "octomap_ros" provides a separate development package or SDK, be sure it
  has been installed.
Call Stack (most recent call first):
  CMakeLists.txt:10 (find_package)

調べてみるとoctomapが無いということなのでapt-getしてみた

$ sudo apt-get install ros-kinetic-octomap

すると最新版が入っている???けれでも解決できない。
結局のところ必要だったのはこっちだった。違いはなんやねん。

$ sudo apt-get install ros-kinetic-octomap-ros

pr2_kinematicsがないと怒られる

多分、rosdep installがうまいこと行けば怒られない。

git clone https://github.com/pr2/pr2_kinematics.git -b kinetic-devel

used to generate cache

CMake Error: The source 
"/home/user/catkin_ws/src/leatherman/CMakeLists.txt" 
does not match the source 
"/home/user/catkin_ws/src/leatherman/CMakeLists.txt" 
used to generate cache.  Re-run cmake with a different source directory.

cacheのCMake.txtが邪魔をしているので削除する必要が有る

cd ~/catkin_ws/build/leatherman
rm CMakeCache.txt

warning

とりあえず無視

CMake Warning at /opt/ros/kinetic/share/cmake_modules/cmake/Modules/FindEigen.cmake:62 (message):
  The FindEigen.cmake Module in the cmake_modules package is deprecated.

  Please use the FindEigen3.cmake Module provided with Eigen.  Change
  instances of find_package(Eigen) to find_package(Eigen3).  Check the
  FindEigen3.cmake Module for the resulting CMake variable names.

Call Stack (most recent call first):
  CMakeLists.txt:10 (find_package)


CMake Warning at /opt/ros/kinetic/share/catkin/cmake/catkin_package.cmake:166 (message):
  catkin_package() DEPENDS on 'eigen' but neither 'eigen_INCLUDE_DIRS' nor
  'eigen_LIBRARIES' is defined.
Call Stack (most recent call first):
  /opt/ros/kinetic/share/catkin/cmake/catkin_package.cmake:102 (_catkin_package)
  CMakeLists.txt:18 (catkin_package)

ここまででとりあえずワーニングは出るがbuildに成功した

build成功後

roslaunch smpl_test goal_pr2.launchができない

ResourceNotFound: sbpl_collision_checking_test

単純な話で$ROS_PACKAGE_PATHに~/catkin_ws/src/smplが追加されていないため。~/.bashrcでexportすれば解決できる。

~/.bashrc
export ROS_PACKAGE_PATH=~/catkin_ws/src/smpl:$ROS_PACKAGE_PATH

追記2019/08/10

https://github.com/aurone/pr2_sbpl_interface_config
これを動かしたいと思った。

cd ~/catkin_ws/src
git clone https://github.com/aurone/pr2_sbpl_interface_config.git

launchファイルを見るとpr2_moveit_configが必要そうだったので追加。

cd ~/catkin_ws/src
git clone https://github.com/ros-planning/moveit_pr2.git
catkin build

とりあえずbuildは成功

cd ~/catkin_ws/src/pr2_sbpl_interface_config/launch
roslaunch demo.launch
demo.launch
ResourceNotFound: pr2_sbpl_interface_config

.bashrcに追加

~/.bashrc
export ROS_PACKAGE_PATH=~/catkin_ws/src/pr2_sbpl_interface_config:$ROS_PACKAGE_PATH

もう一度、demo.launchを動かしてみる

demo.launch
[ WARN] [1565429858.098507537]: Link 'head_mount_kinect_ir_link' is not known to URDF. Cannot disable collisons.
[ WARN] [1565429858.098572130]: Link 'head_mount_kinect_rgb_link' is not known to URDF. Cannot disable collisons.
[ WARN] [1565429858.098596310]: Link 'head_mount_link' is not known to URDF. Cannot disable collisons.
[ WARN] [1565429858.098603891]: Link 'head_mount_prosilica_link' is not known to URDF. Cannot disable collisons.
[ WARN] [1565429858.100868856]: Link 'head_mount_link' is not known to URDF. Cannot disable collisons.
[ WARN] [1565429858.424241902]: The root link base_footprint has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.
[ WARN] [1565429858.615169947]: Manifest not found in folder '~/constraints_approximation_database'. Not loading constraint approximations.

...

[ INFO] [1565429862.261874614]: Listening to '/move_group/monitored_planning_scene'
TIFFFetchNormalTag: Warning, Incompatible type for "RichTIFFIPTC"; tag ignored.
TIFFFieldWithTag: Internal error, unknown tag 0x829a.
TIFFReadDirectory: Warning, Sum of Photometric type-related color channels and ExtraSamples doesn't match SamplesPerPixel. Defining non-color channels as ExtraSamples..
TIFFFetchNormalTag: Warning, Incompatible type for "RichTIFFIPTC"; tag ignored.

...

[ERROR] [1565429863.746551544]: Group 'base' does not have a parent link
[ WARN] [1565429863.746672912]: The root link base_footprint has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.
[ WARN] [1565429864.747415366]: 
Deprecation warning: Trajectory execution service is deprecated (was replaced by an action).
Replace 'MoveGroupExecuteService' with 'MoveGroupExecuteTrajectoryAction' in move_group.launch
[ WARN] [1565429864.768311492]: Interactive marker 'EE:goal_r_wrist_roll_link' contains unnormalized quaternions. This warning will only be output once but may be true for others; enable DEBUG messages for ros.rviz.quaternions to see more details.

先は長い

追記2019/08/13

'head_mount_link' is not known to URDF. Cannot disable collisons.のworning

~/catkin_ws/src/moveit_pr2/pr2_moveit_config/launch/planning_context.launch
<param if="$(arg load_robot_description)" name="robot_description" command="$(find xacro)/xacro '$(find pr2_description)/robots/pr2.urdf.xacro' --inorder" />

ここの部分を以下に変更(kinectのurdfを有効化?)

~/catkin_ws/src/moveit_pr2/pr2_moveit_config/launch/planning_context.launch
<param if="$(arg load_robot_description)" name="robot_description" command="$(find xacro)/xacro '$(find pr2_description)/robots/pr2.urdf.xacro' KINECT1:='true' --inorder" />

すると、'head_mount_link' is not known to URDF. Cannot disable collisons.のエラーは解消された。[1]
あまりxacroファイルの中身を変更したくなかったのでパラメータを与える形にした。

The root link base_footprint has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF.のworning

roscd pr2_description
sudo vim urdf/base_v0/base.urdf.xacro

そして

base.urdf.xacro
    <link name="${name}_footprint">
      <inertial>
        <mass value="1.0" />
        <origin xyz="0 0 0" />
        <inertia ixx="0.01" ixy="0.0" ixz="0.0"
                 iyy="0.01" iyz="0.0" izz="0.01" />
      </inertial>
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <box size="0.01 0.01 0.01" />
        </geometry>

        <material name="White" />
      </visual>
      <collision>
        <!-- represent base collision with a simple rectangular model, positioned by base_size_z s.t. top
             surface of the collision box matches the top surface of the PR2 base -->
        <origin xyz="0 0 ${0.051+base_collision_size_z/2}" rpy="0 0 0" />
        <geometry>
          <box size="0.001 0.001 0.001" />
        </geometry>
      </collision>
    </link>

イナーシャの部分をコメントアウト

base.urdf.xacro
    <link name="${name}_footprint">
      <!--
      <inertial>
        <mass value="1.0" />
        <origin xyz="0 0 0" />
        <inertia ixx="0.01" ixy="0.0" ixz="0.0"
                 iyy="0.01" iyz="0.0" izz="0.01" />
      </inertial>
      -->
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <box size="0.01 0.01 0.01" />
        </geometry>

        <material name="White" />
      </visual>
      <collision>
        <!-- represent base collision with a simple rectangular model, positioned by base_size_z s.t. top
             surface of the collision box matches the top surface of the PR2 base -->
        <origin xyz="0 0 ${0.051+base_collision_size_z/2}" rpy="0 0 0" />
        <geometry>
          <box size="0.001 0.001 0.001" />
        </geometry>
      </collision>
    </link>

多分、製作者もdummylinkのつもりでfootprint linkを作っていると判断した。よってイナーシャも考えなくても良いかなと(あとはオドメトリ関連か?)[2]
本来であればdummylinkを追加すればよいのだが、追加すると他のことで怒られるのでめんどくさくなった。

TIFFFieldWithTag: Internal error

モデルの視覚化に関するエラーでロボットをシミュレートするときにはあまり問題はない。

[ WARN] [1565664107.508820196]: The STL file 'package://pr2_description/meshes/sensors/kinect_prosilica_v0/115x100_swept_back--coarse.STL' is malformed. It starts with the word 'solid', indicating that it's an ASCII STL file, but it does not contain the word 'endsolid' so it is either a malformed ASCII STL file or it is actually a binary STL file. Trying to interpret it as a binary STL file instead.

上記のWARNINGはここを参照にすると解決した。
具体的には該当のSTLファイルをmeshlabで開き、保存しなおした。

パッケージの追加

ここに従って環境を整えてみる。

  • sbpl_geometry_utils
  • moveit_planners_sbpl
  • pr2_common

pr2_commonを追加する際に追加したもの

  • ivcon
  • convex_decomposition

参考文献

[1] https://gist.github.com/davetcoleman/4256083
[2] https://answers.ros.org/question/192817/error-msg-the-root-link_base-has-an-inertia-specified-in-the-urdf-but-kdl/

1
1
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
1
1