Hypothesis-based Belief Planning for Dexterous Grasping††thanks: We gratefully acknowledge support of FP7 grant IST-600918, PacMan.
Belief space planning is a viable alternative to formalise partially observable control problems and, in the recent years, its application to robot manipulation problems has grown. However, this planning approach was tried successfully only on simplified control problems. In this paper, we apply belief space planning to the problem of planning dexterous reach-to-grasp trajectories under object pose uncertainty. In our framework, the robot perceives the object to be grasped on-the-fly as a point cloud and compute a full 6D, non-Gaussian distribution over the object’s pose (our belief space). The system has no limitations on the geometry of the object, i.e., non-convex objects can be represented, nor assumes that the point cloud is a complete representation of the object. A plan in the belief space is then created to reach and grasp the object, such that the information value of expected contacts along the trajectory is maximised to compensate for the pose uncertainty. If an unexpected contact occurs when performing the action, such information is used to refine the pose distribution and triggers a re-planning. Experimental results show that our planner (IR3ne) improves grasp reliability and compensates for the pose uncertainty such that it doubles the proportion of grasps that succeed on a first attempt.
Keywords:Dexterous Grasping Information Gain Belief Space Planning Reach to Grasp Planning
Imagine that you are reaching into the fridge to grasp an object you can only partially see. Rather than relying solely on vision, you must use touch in order to localise it and securely grasp it. However, humans would not poke the object to localise it first and then grasp it. We compensate for the uncertainty by approaching the object in a way such that if a contact occurs it will generate enough information about where the object is and the object will be grasped with a minimum adaptation of the initial trajectory.
Previous work attempted to couple the uncertainty reduction and grasp execution in the framework of partially observable Markov decision processes (POMDPs). Although methods are advancing for continuous state (porta2006point; bai2010monte; brooks2006parametric) and continuous action spaces (porta2006point; murphy2000survey), no POMDP planners yet scale to the high dimensional belief state and action spaces required for robot grasping. This is especially true for manipulators with great dexterity as they have a high number of degrees of freedom. Instead, actual robot implementations of the POMDP approach to active tactile grasping separate exploratory and grasping actions and plan how to sequence them (hsiao2011robust), typically by relying on a user-defined threshold to know when the belief is accurate enough to switch from gathering information to execution of a pre-computed grasp action. This approach fails to exploit the fact that, in tactile grasping, hand movements can both move towards the grasp configuration, and reduce uncertainty. They are most naturally performed concurrently, rather than sequentially. Furthermore, these approaches typically rely on constraining the belief space to Gaussian distributions. Extensions to non-parametric representations of belief (bib:nikandrova_2014), typically result in intractable planning problems due to the high dimensionality of non-Gaussian parametrisation.
This work presents a formulation of dexterous manipulation that aims to exploit concurrency in exploratory and grasping actions for reach-to-grasp hand movements. The properties of the approach are that it:
tracks high-dimensional belief states defined over a 6D, non-Gaussian pose uncertainty;
efficiently plans in a fixed-dimensional space;
simultaneously gathers information while grasping, i.e., there is no need to switch between gathering information and grasping since the action space is the space of dexterous reach-to-grasp trajectories;
does not require a user-supplied mesh model of the object or a pre-computed grasp associated with the object;
copes also with non-convex objects, i.e., there are no limitations to the shape of the objects that it can successfully grasp.
We build our approach by combining the idea of hypothesis-based planning (HBP), initially proposed in (bib:platt_isrr_2011), and our one-shot learning algorithm for dexterous grasping of novel objects (bib:kopicki_2015). The hypothesis-based planner works as follows. Instead of planning directly in a high dimensional belief space, our plan is constructed on a fixed-dimensional, sampled representation of belief. In other words, the belief space is projected onto a set of competing hypothesis in the underlying state space. However, our implementation of the HBP algorithm extends the work in (bib:platt_isrr_2011) in several directions. First, Platt’s formulation of the hypothesis-based planner is defined on a set of actions (i.e., movement constrained in the horizontal plane) that differs from the actual grasp (i.e. a pinch grasp with two paddles). In contrast, we formulate the problem on the same action space for each stage (i.e., dexterous reach-to-grasp trajectories). As a result, we do not require a user-supplied threshold over the current belief to estimate when to interrupt the information gathering phase and execute a pre-defined grasp. Another difference is that the observational model used in (bib:platt_isrr_2011) relies on contactless sensors (i.e. laser sensors), while we maximise tactile observations for a dexterous robotic hand; and, finally, we do not make any assumptions about the model of the object to be grasped, in contrast to the original work that assumes a convex object (i.e. a box) aligned in front of the robot (see Sec. LABEL:sec:grasp_planning for further details). On top of our hypothesis-based planner, our grasping algorithm enables us to learn a set of grasp contacts on a point-cloud object model (PCOM), directly obtainable from a depth camera, and to generalise to objects of novel shape. Therefore we do not require a mesh model of the object, and we can also generate grasps on target objects with incomplete point clouds. Hence our algorithm is exceptionally flexible in planning dexterous reach-to-grasp trajectories for novel objects.
In order to link these two methods, hypothesis based planning and dexterous grasping of novel objects, we need to construct a representation of the belief space that will allow us to track pose uncertainty for a PCOM, in 6D, and cope with the non-Gaussian posterior. We do so by employing a non-parametric representation of the belief state defined as a kernel density estimator. Each kernel is a weighted pose of the target object inferred from visual data collected on the fly (see Sec. LABEL:sec:belief_state_estimation).
Our experimental results show that our planner, IR3ne, is more reliable than open-loop grasping from vision. We further show that IR3ne improves over simple tactile re-planning in three ways: i) it doubles the proportion of times in which the robot reaches and grasps the object at the first attempt, ii) if an unexpected contact is generated along the trajectory, the contact provides more information about the object’s pose, and thus iii) it reduces the number of replanning steps before converging to a grasp. Experiments in simulation and on a real robot (Fig. 1) confirm these attractive properties.
We continue with related work (Sec. LABEL:sec:background), then describe the planning problem formulation (Sec. LABEL:sec:problem). We describe all aspects of the framework (Sec. LABEL:sec:implementation), and report and discuss the experimental results (Sec. LABEL:sec:experiments). We finish with concluding remarks (Sec. LABEL:sec:conclusion).