Examples with smpl

[shenlirobot]

Hi @aurone :

Thank you so much for open sourcing this awesome planning software!

I am trying to do manipulation with SBPL.

By following your instruction, I was able to build smpl, leatherman, moveit_planners_sbpl, pr2_sbpl_interface_config, and ur5_sbpl_interface_config.

Then I was trying to run some examples.

---

I did $ roslaunch smpl_test goal_pr2.launch but it does not seem to do things at least from my terminal. This is the output:

NODES
  /
    call_planner (smpl_test/callPlanner)

auto-starting new master
process[master]: started with pid [25875]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to ae9668a0-40eb-11e8-b363-7085c23bebfb
process[rosout-1]: started with pid [25888]
started core service [/rosout]
process[call_planner-2]: started with pid [25905]

Do I need to run RVIZ or other modules?

---

Then I tried pr2_sbpl_interface_config. I changed the move_group path to $(find pr2_sbpl_interface_config)/launch/move_group.launch and I was able to get OMPL running in moveit plugin on RVIZ. But when I chose left_arm and clicked plan, I got this error [ERROR] [1523824813.250117840]: No group '' found in the Robot Collision Model.

---

I also tried ur5_sbpl_interface_config. Similarly, I changed the move_group path to $(find ur5_sbpl_interface_config)/launch/move_group.launch. But when I launched roslaunch ur5_sbpl_interface_config demo.launch, I got this error:

[ INFO] [1523824926.162820383]: CollisionRobotSBPL(const RobotModelConstPtr&, double, double)
[ WARN] [1523824926.167659051]: No key 'world_joint' found in robot collision model config
[ WARN] [1523824926.167774927]:   Default to { name: world_joint, type: floating }
[ WARN] [1523824926.168020549]: No key 'voxels_models' found in robot collision model config
[ERROR] [1523824926.182473746]: Failed to find 'self_collision_model' key on the param server
terminate called after throwing an instance of 'std::runtime_error'
  what():  Failed to find 'self_collision_model' key on the param server
[move_group-2] process has died [pid 1665, exit code -6, cmd /home/shen/mit/vaibhav/devel/lib/moveit_ros_move_group/move_group --debug __name:=move_group __log:=/home/shen/.ros/log/74a7a1de-40ed-11e8-a465-7085c23bebfb/move_group-2.log].
log file: /home/shen/.ros/log/74a7a1de-40ed-11e8-a465-7085c23bebfb/move_group-2*.log

I am wondering whether there are any packages I missed or whether there are any examples that I can try?

Thank you so much!

[aurone]

Hi @Shentheman,

For the goal_pr2.launch example, you probably want to add the --screen argument to roslaunch to see the terminal output. Additionally, you can run RViz and add a MarkerArray display on topic "/visualization_markers" to see an animation of the path.

That error message for the second issue isn't super useful. Are you using the 'MotionPlanning' display in RViz to send plan requests to MoveIt? I don't use that interface, but my guess is that the planner id isn't being set to an existing configuration. Unlike the ompl plugin, the smpl plugin doesn't make any attempt to choose a reasonable default planning algorithm to use for a joint group when no planner id is selected. For example, you would need to specify something like "right_arm[ara_bfs_ml]" as the planner id to plan for the "right_arm" group. This package doesn't have left_arm configuration, but you can copy the config for right_arm.

I'm not sure what state the UR5 config is in. The group that was using it opted to use FCL instead of the sbpl_collision_checking package (related to that error). An easy solution (or if you just want to go with FCL as the collision checker) is to remove the "" from move_group.launch.

Hope that helps!

Andrew

[shenlirobot]

Thank you so much @aurone !!! Your reply is very helpful!

I just tried running SMPL via both pr2_sbpl_interface_config and smpl_test. I got both of them working! The planner is awesome! I wrote a brief description about what I did. I just want to share with you if you want to make a tutorial in the future :) or other people want to try more things.

I also have some questions in bold if you could share with me some hints :)

Usage

Use SMPL MoveIt! Plugin

• roslaunch pr2_sbpl_interface_config move_group_pr2.launch
• roslaunch pr2_sbpl_interface_config moveit_pr2.launch
• roslaunch pr2_sbpl_interface_config moveit_rviz_pr2.launch
• ???I am not sure whether it is correct??? Since SBPL does not use FCL for collision checking, but uses sbpl_collision_checking, we cannot use the regular moveit_ros_visualization panel for motion planning. We have to use a customized panel moveit_planners_sbpl.
• In RVIZ, first change the Fixed Frame in Global Options to odom_combined
• Then go to the top horizontal menu bar - Panels - Add New Panel - moveit_planners_sbpl - Move Group Command Panel.
• Add topics /phantom_comtrols - /update - InteractiveMarkers and /visualization_markers - MarkerArray
• Inside the panel, input /robot_description in the Robot Description field and click Load Robot.
• Choose ID as right_arm[adaptive_planner] or others for right_arm.
  • Left arm is not ready
• Under Position Command, choose right_arm as Joint Group.
• Drag the arm around via the interactive markers
  • Make sure that the robot goal pose is inside the red box defined by Workspace Parameters. Otherwise error will raise [ERROR] [1523884840.672022604]: Heuristic goal is out of BFS bounds [ERROR] [1523884840.733679242]: Failed to plan within alotted time frame (10.00 seconds, 0 expansions)
• Click Plan to Position
  • It might have error messages [ERROR] [1523884866.276634897]: Found empty JointState message
• Then a plan will show up
• ???What is the difference between Plan to Position and Plan to Configuration???
• ???Move to xxx seems to not work? Maybe I did not get the planner configurations correct???

Use SMPL Alone

• roslaunch pr2_sbpl_interface_config moveit_pr2.launch
• roslaunch pr2_sbpl_interface_config moveit_rviz_pr2.launch
• roslaunch smpl_test goal_pr2.launch --screen
• In RVIZ, first change the Fixed Frame in Global Options to odom_combined
• Add topics RobotModel and /visualization_markers - MarkerArray Then on RVIZ, you will be able to see the plan and workspace grid.

[aurone]

Looks good. For your questions:

• The custom RViz plugin isn't strictly necessary to use the smpl planner, the standard c++/python interfaces should work as well. The reason for the custom plugin is that the moveit_ros_visualization one defaults to planning for goal states specified as the full joint configuration. While this is the default behavior in ompl, it's not the default behavior in smpl (though it works to an extent). The reason for this is that it's more straightforward to design useful heuristics (like the BFS heuristic) if they only have to provide enough information to guide the search towards "under-specified" goal poses. The typical use case for us is to plan to grasp poses, which doesn't require the full goal joint state to be known. It also gives the search more freedom to find a goal state without sampling states up front via IK.

• Related to the first question, the smpl plugin can be run with FCL without adjustment. Just clear the "/collision_detector" parameter or remove it from the move_group.launch launch file. Note the FCL library will be much slower, but it won't suffer from the errors inherent in sbpl_collision_checking from using an approximate model. :)

• Plan to Position is the 'goal pose for the tip link' command, while Plan To Configuation is the full goal joint state command.

• Move to XXX should be identical to Plan to XXX, aside from setting the 'plan_only' flag internally, which triggers MoveIt's execution pipeline. You may need to take additional steps to hook MoveIt up to controllers on the robot or simulated controllers (pr2_moveit_config provides much of this configuration).

Thanks for taking the time to clear some of this up for others. smpl is lacking in tutorials at the moment.

Andrew