Winning with MoveIt!

Team Delft – Amazon Picking Challenge

Mukunda Bharatheesha
08.10.2016

Self Introduction

Robotics InstituteStowing Challenge

Picking Challenge

Delft - 214

Delft – 105 (00:00:30)

NimbRo - 186

PFN – 105 (00:01:07)

MIT - 164

NimbRo - 97

2

Contents●●

Robotics InstituteGetting started with the Amazon Picking
Challenge (APC) [1]Team Delft APC motion module with MoveIt!
[2]●

MoveIt! Specific lessons learnt●

General lessons learnt●

Concluding remarks

3Getting started with APC

Robotics Institute4Why MoveIt!?
●●●

Robotics InstitutePrevious experience with pick and placeapplications.

Knowledge of using pick and place pipeline
for an industrial “boxpacking” task.

Curiosity! [3]5Robot setup
Workspace analysis of different configurations●●Robotics Institute

Fixed Configuraton● Upper and lower binsat the centre.
● Deep corner bins.
Vertical configuration● Side and corner bins.
● Practicalities with therail.

6

Robot Setup
Selected configuration

Robotics Instituteroslaunch moveit_workspace_analysis
workspace_analysis.launchroslaunch moveit_workspace_analysis
reader.launch7

Basic Software ArchitectureHigh level task manager
SenseObjectRecognition

PlanGrasp
Synthesis

ObjectPose
Sense
Estimation

Bin Pose
Estimation
Robotics InstituteActCoarseMotion

FineMotion
Manipulation
PlanningI/O

8

Motion Module - Manipulation●●●Robotics Institute

Grasp pose scoring● Perpendicular distance to CoM.
● Approach direction of the robot.
● Preferred grasping direction.
Grasp strategy based MoveGroups● Top Suction.
● Front Suction.
● Pinch.
Final grasp pose selection● Highest scoring grasp pose
(the_chosen_one).
● Collision check based pruning(moveit_msgs::GetPositionIK).

9Motion Module – Robot motionRobot Motions
OfflineOnlineCoarse Motions

Fine Motions

Free (joint) spacemotion planningwith RRT-Connect.

Cartesian path
planning to achieve
final grasp pose.

(Trajectory Cache)●●

Robotics Instituterobot_state::RobotState APIs for setting various“MasterPoses”.
planning_interface::MoveGroup APIs forplanning.

10Motion Module – I/O (1)●●Robotics Institute“Event” based I/O● Key waypoints - “approach”, “contact”, “lift”,“retreat”● I/O actions - “vacuum on”, “suction on”, “frontsuction activate”, …
Reliable trajectory tracking● Track trajectory waypoints (distance towaypoints).
● Stable tracking regardless of other tasks.
● Dedicated callback queue and backgroundspinner.
11Motion Module – I/O (2)●●●Robotics InstituteRequires additional “AsyncSpinner”● MoveGroup::execute()already runningan AsyncSpinner.
New SimpleAsyncSpinner● No dependencies besides ROS and standard
library.
● Lock-free using atomic variables.
Multiple AsyncSpinners now supported in
ROS-Kinetic [4]12Lessons Learnt
1. Collision checking redundancy“Gap” in octomap due to reflections
Robotics Institute13Lessons Learnt
1. Collision checking redundancy●

Bin collision models

Collision checking with “new” meshes in
the planning scene● Non-reproducible but random failuresin collision checking.
● Add sanity checks for “impossible”
motions.
●

Robotics Institute14

Lessons Learnt
2. Octomap – Planning Scene●●

Robotics Institute“Static” point cloud● One point cloud snapshot instead of a point
cloud “live feed”.
● Simulated point cloud stream using a topic.
MoveGroup::getCurrentState()takes
long time to return while using octomaps● Use robot_state::JointModelGroupto avoid delays when real robot state isdifferent from “test” robot state during
online planning.
● Cause: Planning scene monitor prioritises
servicing octomap updates(*)
15

Lessons Learnt
3. Octomap clearance●●Robotics Institute“Static” point cloud● One point cloud snapshot instead of a point
cloud “live feed”.
● Simulated point cloud stream using a topic.
/clearOctomap()- Inconsistency withclearing the current octomap.
● Publish “out of range” valued point cloud.
● Octomap always cleared on service call.

16

Lessons Learnt
4. Trajectory stitching●●●Robotics InstituteCombine fine motions with coarse motions● IK solution need not necessarily match withcoarse motion start.
● RRT-Connect from last IK point to“MasterPose”
Trac-IK with “distance” setting● ROS parameter namespace issues.
Possibly stitch entire trajectory to destination.
● Combine multiple motion segments in jointspace.
● Re-(time) parameterize the “stitched”
motions in time
(iptp.computeTimeStamps()).

17

Lessons Learnt
5. I/O as joints?

●●Robotics InstituteCommon practice● Define various I/O as joints with a very smallrange.
● “Plan” solutions to command I/O and use
actionlib interface.
Our approach via external trajectory
monitoring● Dependency on /joint_states topic.
● Dedicated I/O support possibly the way to goforward?

18

Lessons Learnt
Behavior design●●●Robotics InstituteInterspersed Plan-Act● Robot stuck in the bin.
● Design adequate and appropriate recovery
behaviour.
Fake components● Provide lots of room for testing.
● Clarity in interface definitions.
● Easy integration when real components areavailable.
Writing “json” files correctly19

Lessons Learnt
Assumptions●●●

Optimal planners● Euclidean distance NOT default.
● Effort-based, time-based, etc.
Trac-IK cost functions● Distance● Speed● Manipulability
Robot Driver*● Sanity checks● Joint naming“Assumptions bear the roots of all disasters!”

Robotics Institute“Gravity is a heartless entity!”
20

Concluding Remarks

How MoveIt! isdesigned to beused.

How we used it!
Image Sources:
www.dx.com
www.northerntools.com

Robotics Institute21

Concluding Remarks

Robotics Institute20 days before the challenge!
(No stowing yet!)22Concluding Remarks●●●Robotics InstituteStowing completed in almost half of allottedtime.
More than 16 picks performed in the given time(including “move jobs”)
Not moving like Icebergs anymore!

23

Acknowledgements

Robotics InstituteMade possible by:

Robotics Institute24Thank you for yourattention!

Robotics Institute25References[1] www.amazonpickingchallenge.org[2] moveit.ros.org[3]
MoveIt! - Strengths, Weaknesses and Developer Insights, Dave T. Coleman, RosCon 2015, Hamburg.
[4] AsyncSpinners

Robotics Institute26