Generative grasp synthesis from demonstration using parametric mixtures
Ermano Arruda Claudio Zito Mohan Sridharan Marek Kopicki Jeremy L. Wyatt




Controllability of Kronecker Product Networks
    [
    December 30, 2023
=============================================








We present a parametric formulation for learning generative models for grasp synthesis from a demonstration. We cast new light on this family of approaches, proposing a parametric formulation for grasp synthesis that is computationally faster compared to related work and indicates better grasp success rate performance in simulated experiments, showing a gain of at least 10% success rate (p < 0.05) in all the tested conditions. The proposed implementation is also able to incorporate arbitrary constraints for grasp ranking that may include task-specific constraints. Results are reported followed by a brief discussion on the merits of the proposed methods noted so far.













§ INTRODUCTION


We address the problem of modelling generative models for grasp synthesis using parametric representations for probability densities. In recent decades, considerable effort has been put towards tackling the problem of grasping <cit.>.







Generative approaches for grasp synthesis have demonstrated to be more versatile since they can be used to generate complete hand configurations in novel contexts, e.g. <cit.>.

Kopicki et al <cit.> have proposed a non-parametric technique to learn generative models from demonstration for dexterous grasping via Kernel density estimation (KDE). Such approach makes use of KDEs to approximate probability densities over the special Euclidian group - SE(3), as well as hand configuration models encoding the shape of the hand for a given demonstration. The original approach has been shown to be able to generalise demonstrated grasps to novel query object point clouds, without requiring prior knowledge about object pose, shape (such as CAD mesh models) or dynamic properties such as friction coefficients, mass distribution, among others.

Although KDEs are very appealing, one drawback of learning such non-parametric models is the computational time when evaluating the likelihood of samples. If a KDE has K kernels and one wishes to evaluate the likelihood of N data points under the model, the time complexity for computing the data likelihood is O(K N). Noting that for KDEs K is typically large, in the order of hundreds, since commonly a kernel is placed on every training data point. This large number of kernels makes the final time complexity dependent on the size of the training data set used to approximate a desired density. In contrast, parametric mixtures usually need a smaller number for K to effectively approximate a probability density. Once the model parameters are learnt via Expectation Maximisation (EM) <cit.>, the runtime for evaluating the likelihood of query data points is not dependent on the size of the data set used for training, but only on the fixed number chosen for K. Thus, although the complexity for data likelihood computation is the same in principle, in practice parametric mixtures are orders of magnitude faster than KDEs since K is rarely greater than 10 for most scenarios. 



This work re-interprets the approach in <cit.> for grasp synthesis using a parametric mixture formulation for density modelling. We show the benefits for grasp generation time and performance in simulation under different sensor noise conditions, also showing deployment on a robot platform. 






First, we proceed to describe the basic representations utilised throughout this paper for modelling probability densities, followed by the description of our approach. Finally, we present our experimental results in simulation and show preliminary deployment of the approach on a robot platform.


























§ REPRESENTATIONS


We are interested in modelling joint probability densities over rigid body transformations belonging to the SE(3) and arbitrary feature vectors in ℛ^n. Thus, realisations of the random variable we intend to model lie in the space 𝐱∈ SE(3) ×ℛ^n. Concretely, the special Euclidean group is the cartesian product of members of the special orthogonal group SO(3) (the group of rotations) and members of ℛ^3 representing translations, thus SE(3) = ℛ^3 × SO(3). If we choose to represent rotations with unit quaternions, then 𝐱=(𝐩, 𝐪, 𝐫) is a d=7 + n dimensional vector, where 𝐩∈ℛ^3, 𝐪∈ℛ^4 and 𝐫∈ℛ^n. In this work, the same type of feature used by <cit.> will be utilised, which are the eigen-values k_1 and k_2 of the principal curvatures for a given point 𝐩 from an object point cloud. In Fig. <ref> this type of feature is illustrated. Thus, concretely, the feature vector is represented as 𝐫=[k_1,k_2] ∈ℛ^2.

We will see that we are able to learn probability densities over this manifold using data acquired from demonstration where a data set 𝒟 is acquired, such that



    𝒟 = {𝐱_𝐣 | 𝐱_𝐣∈ℛ^3 ×𝒮𝒪(3) ×ℛ^n }_j=1^J_D


where 𝐱_j ∼𝐩𝐝𝐟(𝐩, 𝐪, 𝐫).

In particular, we will represent quaternions noting their relationship with angular velocities w∈ℛ^3 through the logaritimic and exponential maps defined by Eq. <ref> and <ref>, as similarly noted by <cit.>. Given a unit quaternion 𝐪 = [𝐪_v, q_w]^T, we define its logarithmic map as:




    ω = log(𝐪) = {[ arccos(q_w) 𝐪_v/𝐪_v              𝐪_v ≠0;           [0,0,0]^T           otherwise ].




And conversely, the exponential map is given by:



    𝐪 = exp(ω) = {[ [sin(ω ) ω, cos(ω )]^T                    ω≠0;            [0,0,0,1]^T              otherwise ].



Using these mappings, we will fit parametric mixtures to model joint distributions over 𝐱 = (𝐩, ω, 𝐫), where the quaternion representation for ω is readily given by the exponential map 𝐪 = exp(ω).



 §.§ Probability density approximation using parametric mixtures


The density 𝐩𝐝𝐟(𝐱) is approximated using a parametric Gaussian mixture model (GMM) modified so as to internally take care of the appropriate exponential and logarithmic mappings for quaternions. It is then defined as follows:



    𝐩𝐝𝐟(𝐱) = ∑_j=1^K w^j 𝒩(𝐱 | μ_x^j, Σ_x^j)




For convenience, let 𝐮=(𝐩,𝐪), i.e. a rigid body transformation. Thus, with Σ_x^j^-1 = Λ_x^j, we denote:




    μ_x^j    = (μ_u^j, μ_r^j), 
    Σ_x   = [ Σ_uu^j    Σ_ur^j  
    Σ_ur^j    Σ_rr^j ] , Λ_x^j = [ Λ_uu^j    Λ_ur^j  
    Λ_ur^j    Λ_rr^j ]
















In this work the density p( 𝐮 | 𝐫) is a conditional probability density of rigid body transformations given a contact feature 𝐫, and is also modelled as a Gaussian mixture model. The parameters of this conditional mixture are obtained in closed form from Eq. <ref> as (see <cit.> for an overview):




    μ_u|r^j    = μ_u^j - Σ_uu^j Λ_ur^j (𝐫 - μ_r^j),  
      
    Σ_u|r^j    = Σ_uu^j 
      
         p^j(𝐮 | 𝐫)    = 𝒩(𝐱 | μ_u|r^j , Σ_u|r^j) 
      
    π^j    = w^j 𝒩(𝐫 | μ_r^j, Σ_rr^j)/∑_j=1^K𝒩(𝐫 | μ_r^j, Σ_rr^j)




Therefore, p( 𝐮 | 𝐫) is expressed as  


    p( 𝐮 | 𝐫) = ∑_j=1^Kπ^j p^j( 𝐮 | 𝐫)




The final density over features 𝐫, i.e. p(𝐫), is also modelled as a Gaussian mixture and is obtained via marginalisation of Eq. <ref> with respect to 𝐮 such that:




    p^j( 𝐫) = ∫𝒩(𝐱 | μ_x^j, Σ_x^j) du = 𝒩(𝐫 | μ_r^j, Σ_r^j)



Thus, it follows that



    p(𝐫) = ∑_j=1^K w^j p^j( 𝐫).




The mixture in Eq. <ref> is learned using the expectation maximisation algorithm (EM) over contact data acquired from a grasp demonstration 𝒟= {𝐱_𝐣 | 𝐱_𝐣∈ℛ^3 ×𝒮𝒪(3) ×ℛ^n }. The mixture formulation is more compact and less memory hungry. Once the density is learnt, we only need to keep its learnt parameters in memory and compute in closed form the additional probability densities in Eq. <ref> and Eq. <ref>. In addition, it needs fewer kernels to approximate the densities, therefore K is typically smaller than 10. In contrast to the KDE approach, each data point becomes a the center of a kernel, which is of the order of hundreds per contact model learnt.


 



§ GRASP SYNTHESIS USING PARAMETRIC CONTACT MODELS


Having defined the basic representations for learning distributions using parametric mixtures, we proceed to describe the main components of what we refer to as the parametric grasp synthesis approach.

We will be learning joint probability densities over 𝐱=(𝐩, 𝐪, 𝐫). For compactness we will refer to the position and orientation (pose) of a feature 𝐫 as 𝐯=(𝐩, 𝐪), and therefore 𝐱 can be written as



    𝐱 = (𝐯,𝐫),



where 𝐯∈ SE(3) and 𝐫∈ℛ^n is a feature vector.







 §.§ The object representation
 At all times, the object intended to be grasped is represented by O(𝐯, 𝐫). Concretely, it consists of the current object point cloud augmented with features 𝐫 = [k_1, k_2] representing the principal curvatures at each point of the object. Finally, for a given point 𝐩 from the point cloud, using corresponding eigenvectors of the principal curvatures and estimated normal at this given point, one can construct a frame 𝐯 = (𝐩,𝐪). Hence, the object point cloud is then augmented to be a cloud of rigid body transformations associated with a features 𝐫 as depicted by Fig. <ref>. 






 §.§ Contact model


For a given hand link L_i, the conditional contact model density M_i(𝐮|𝐫) is modelled as a conditional probability density over finger link poses 𝐮∈ SE(3) given contact surface features 𝐫∈ℛ^n. This density is proportional to the likelihood of finding a finger link located at 𝐮 with respect to the location 𝐯 of a contact point with feature vector 𝐫. Such model is constructed from the dataset 𝒟_O = {𝐱_j}_j=1^K_O as follows. Let 𝐬_i = (𝐩_i, 𝐪_i) ∈ SE(3) be the pose of the link L_i in world frame. The relative pose of link L_i with respect to a feature pose 𝐯_j is given by



    𝐮_ij = (𝐩_𝐢𝐣,𝐪_𝐢𝐣) = 𝐯_j^-1∘𝐬_i



Thus we are able to construct the contact model representing a probability distribution of relative poses with respect to contact features 𝐫_j as



    M_i(𝐮|𝐫) ≈ p( 𝐮 | 𝐫)



where p( 𝐮 | 𝐫) is defined in Eq. <ref> and is obtained via conditioning Eq. <ref> learnt using the demonstration data set 𝒟_O.

Furthermore, it is worth noting that M_i is the contact model for link L_i in proximity during demonstration to a set of features modelled by



    M_i(𝐫) ≈ p( 𝐫)



where M_i(𝐫) is obtained via marginalisation of Eq. <ref> as described by Eq. <ref>. The mixture in Eq. <ref> is learnt with EM using the demonstration data set 𝒟_O = {𝐱_j}_j=1^K_O, a data set consisting of data points 𝐱_j = (𝐯_j,𝐫_j) in proximity to finger link L_i.

Let (ṽ, 𝐫̃) ∼ O(𝐯, 𝐫) be a sample from the object intended to be grasped represented by O(𝐯, 𝐫). It follows that if were to compute the data likelihood of features 𝐫̃ over O(𝐯, 𝐫), then features with high data likelihood reflect higher affinity that this finger link L_i could be positioned over 𝐫̃ located at ṽ. The location of this finger link in world frame is given by 𝐬̃ = ṽ∘ũ, where ũ∼ M_i(𝐮|𝐫̃), together with its weight representing this affinity w̃ = M_i(𝐫̃), forming the n-uple (𝐬̃,w̃). 




Depending on the probability distribution of features during demonstration, different finger links show different affinities, thus modelling different contact distributions. Figure <ref> 
depicts this property for two different links of a WSG 50 Schunk gripper. For instance, in Fig. <ref> (left), the link number L_2 is closer to flat patches of surface. By evaluating M_2(𝐫̃) over (ṽ, 𝐫̃) ∼ O(𝐯, 𝐫), one is able to determine the regions of high and low data likelihood/affinity over the object surface represented by O(𝐯, 𝐫), which in this case is a soup box. In Fig. <ref> (right), the same process is repeated this time for L_1 . Note that L_2 prefers flat surfaces, whereas L_1 prefers corners, due to its closer proximity to features describing the edges of the object during demonstration. The features we chose to utilise are the same features described by <cit.>, which are the principal axis of curvature, computed using a PCA method. Thus, each feature 𝐫∈ℛ^2 is a two dimensional vector containing the two largest eigen values approximating the curvatures of a given point of the object point cloud.





 §.§ Hand configuration model


A probability density constructed from a demonstrated grasp trajectory containing starting and final joint configurations of the robot hand. Here, this model is constructed as originally proposed by <cit.>. Denoting 𝐡^t_e as the joint angles at the start of a given demonstrated grasp (pre-grasp configuration), and 𝐡^t_g the joint angles at contact with the object. Finally, let the set of hand configurations be ℋ_c = {𝐡(γ) : γ∈ [-β, β], β∈ℛ^+}, where 𝐡(γ) = (1 - γ) 𝐡^t_g + γ𝐡^t_e, the probability density C(𝐡_c) is then non-parametrically approximated from this set via a KDE as



    C(𝐡_c) = ∑_γ∈ [-β, β] w(𝐡_c(γ)) 𝒩_D(𝐡_c|𝐡_c(γ),σ_c)



where w(𝐡_c(γ)) = exp(-α𝐡_c(γ) - 𝐡^t_g ) and α∈ℛ^+.



 §.§ Grasp synthesis for novel objects
 



  §.§.§ Contact query density


From the learnt densities described above, given a query point cloud for a new object in its augmented representation O(𝐯, 𝐫), a so called contact query density Q_i(𝐬) can be constructed for each link L_i. This probability density is constructed by first sampling (ṽ, 𝐫̃) ∼ O(𝐯, 𝐫) from the object intended to be grasped. Next, 𝐮̃_i ∼ M_i(𝐮|𝐫) is sampled from our learnt contact model for finger link L_i in Eq. <ref>, such that a set of N_Q_i samples 𝐬̃_ij = 𝐯̃_j ∘𝐮̃_i = (𝐩̃_ij,𝐪̃_ij) is constructed 𝒟_Q_i = {𝐬̃_ij}_j=1^N_Q_i and used to fit a query density defined as 



    Q_i(𝐬) ≈∑_k=1^K_Q_i w_k𝒩(𝐬 | μ_s^k,Σ_s^k)



This is density is once again learnt as a GMM using the EM algorithm. 

In this fashion, the set of contact query models, one for each finger link and for a demonstrated grasp g, is denoted by 𝒬_g={Q_i(𝐬)}∀_i.



  §.§.§ Grasp sampling


The query density described above allows one to sample finger link poses over the given query point cloud surface. To construct a full hand posture, first a link pose is sampled using Eq. <ref>, i.e. 𝐬̃∼ Q_i(𝐬). Finally, the full hand posture of a hand is completed using forward kinematics by sampling the remaining joint angles for the remaining links from the hand configuration model C(𝐡_c). Many grasp candidates can be generated in this fashion.



  §.§.§ Grasp optimisation


The generated grasp samples can be later optimised via a derivative-free method using the product of experts likelihood criterion as energy function, such as simulated annealing as done in <cit.>.

We denote a grasp solution as the tuple 𝐡 = (𝐡_w, 𝐡_c) containing respectively the hand wrist pose 𝐡_w ∈ SE(3) and hand joint configuration 𝐡_c ∈ℛ^D, such that if fk(·) is the forward kinematic function of the hand, then the hand links in workspace are given by s_1:N_L, where N_L is the number of links of a robotic hand. Thus, we have



    s_1:N_L = fk(𝐡), s_l = fk_l(𝐡).



The basic optimisation criterion is given by:



    ℒ(𝐡) = C(𝐡) ∏_Q_i ∈𝒬 Q_i(fk_i(𝐡))





    *𝐡 = *arg max_𝐡=(𝐡_w,𝐡_c) = ℒ(𝐡).




In <cit.> an additional expert is defined W(𝐡) so as to penalise collisions in a soft manner, as it was found to prune the solution space to contain better solutions. Collisions are penalised exponentially by the degree of penetration through the object point cloud by any of the hand links. We have also employed a collision expert in the implementation of this work. 

The objective function with the additional collision expert is then given by



    ℒ(𝐡) = W(𝐡) C(𝐡) ∏_Q_i ∈𝒬 Q_i(fk_i(𝐡)).



And again, the best grasp is found by optimising



    *𝐡 = *arg max_𝐡=(𝐡_w,𝐡_c)ℒ(𝐡)



where W(·) is the collision expert, C(·) is the hand configuration expert defined in Eq. <ref>, and Q_i(·) is the contact query expert for link L_i defined by Eq. <ref>. For a robot hand and a given grasp demonstration g, there is a set of contact query experts 𝒬_g. This optimisation criterion, therefore, tries to maximise the product of experts <cit.>, where each expert is a probability density. Each individual expert is responsible for assigning high likelihood to candidate grasps that satisfy just one of the constraints. 

In this work we will explore the mixture of experts approach even further and allow our optimisation criterion to incorporate multiple experts to represent arbitrary task constraints. This will give the potential to generate grasps specifically tailored for a given task. For this purpose let ℰ = E_k(·)_k=1^N_k, such that each E_i(·) assigns a probability to a grasp solution 𝐡 that is close to one if the grasp satisfies the constraint represented by E_i(·) and close to zero if the constraint is not satisfied. This set of constraints can be a set of constraints defined a priori by a human for a given task or learnt autonomously by the robot manipulator. We augment our optimisation criterion as follows:



    ℒ(𝐡) = W(𝐡) C(𝐡) ∏_Q_i ∈𝒬 Q_i(fk_i(𝐡)) ∏_E_i ∈ℰ E_i(𝐡)










Furthermore, we define a hard constraint as an expert that returns binary values of one or zero, accepting or rejecting instantly a grasp, thus implementing rejection sampling. Continuous experts whose image are on the continuous interval [0,1] are referred to as soft constraints in this paper.

 

With this extension, we have then created a framework that allows a great variety of constraints to be incorporated into the grasp generation pipeline. For instance, one can think of experts that imbue dynamic constraints to grasps or specific kinematic constraints such as manipulability, thus paving the way towards task oriented grasp synthesis.




Thus far in this work, the following constraint experts have been defined: i) hard kinematic constraint and ii) soft principal axis alignment constraint. The former expert returns zero if a given grasp solution 𝐡 is not kinematically feasible by the robot platform, and one otherwise. It implements a hard constraint over the grasp generation process, rejecting grasp samples that are not kinematically feasible. The latter expert implements a soft constraint that returns a probability close to one if the wrist frame of the generated grasp 𝐡 is aligned with a given axis provided by the user. This is useful to encourage grasps that always pick an object from the top, for instance.



§ EXPERIMENTS


We perform two separate experiments. The first experiment compares the success rate between the KDE and GMM-based success rate in simulation (we used Bullet physics <cit.>) in four conditions: A) without depth sensing noise and with optimisation, B) with simulated depth sensing noise and with optimisation, C) without noise and without optimisation and D) with noise and without optimisation. The second experiment shows the runtime performance for each respective method when generating 200 hundred grasps for an object, results are shown in Fig. <ref>.

For all experiments and grasp synthesis methods, the optimisation utilised was the derivative-free simulated annealing method with 100 iterations and maximum initial temperature of 1.0 which decays linearly over the number of iterations.

We adopted the same depth sensor noise model as in <cit.>. The noise model combines additive and lateral shift Gaussian noise applied to every pixel in a simulated depth image:


    z̃(u,v) = z(u + 𝒩(0, σ_p^2), v + 𝒩(0, σ_p^2)) + 𝒩(0, σ_d^2),


where σ_p is the standard deviation for the noisy lateral shift of pixels, which has been set to 1 for these experiments. And σ_d is the standard deviation for depth measurements, which was set to 1mm.



 §.§ Performance comparison


For the comparison, we use a parallel gripper (WSG 50 Shunk Gripper) and a single demonstrated grasp on a box, from which object the contact models are learnt for both KDE and GMM-based methods. The KDE-based method has utilised one kernel per data point to approximate the densities. The GMM-based contact model has approximated Gaussian mixture densities with K=2, given the bi-modality of the demonstration as depicted in Fig. <ref> 
and with the number of kernels K_Q=5 for GMM-based query density approximation from Eq. <ref>. 

A total of 60 objects were used for testing the performance of each respective method. Objects are varied, some objects have box-like features, others are curved, cylindrical, convex, non-convex, and of combinations of sizes and shapes as shown in Fig. <ref>.
Objects are always placed in the same pose for both approaches, and simulation parameters are kept fixed. For grasp generation, simulated point clouds are acquired from 4 fixed camera poses around the object, thus obtaining a full 3D point cloud of the object. The acquired point clouds are stitched together and post-processed identically for both approaches, including principal curvature feature computation with search radius set to 1cm.

For each respective approach (KDE and GMM-based), a total number of 200 grasps are generated per object. Sampled grasp solutions are arranged in decreasing order of value as given by Eq. <ref>, in which the highest value represents the best grasp in the sequence. For each approach, the best grasp is executed on each object. Before every execution attempt the object is restarted to the same initial pose.
This process is repeated 10 times over the set of 60 object and results are averaged over these 10 trials. We measure success rate as the proportion of successful grasps over the total number of objects.

Finally, a two-tailed paired t-test is performed to assert the significance of the results between KDE and GMM-based methods over the 10 repeated trials.





 §.§ Preliminary tests on robot platform


We have preliminarily deployed the proposed approach on a real robot platform as shown by Fig. <ref>. For the robot platform, we made use task-specific kinematic constraints, such as kinematic feasibility and axis alignment that can be straightforwardly added in our implementation using Eq. <ref> to prune the grasp synthesis process. A summary video on the Boris platform is available on <https://youtu.be/9RqZaTAH4Fs>.




 §.§ Results




The results in all four different conditions indicate that the GMM-based method shows superior performance over the KDE-based method (p < 0.05) in this setup. We see that in the absence of noise, the final optimisation step has little effect in grasp performance as shown by Fig. <ref> and <ref>. In contrast, optimisation seems to become crucial under noisy conditions as depicted by Fig. <ref> and <ref>, which seems natural. Under the influence of noise, local optimisation with simulated annealing is expected to help to compensate from imperfect sensing, hence still converging to a local or global minimum under the product of experts objective function in Eq. <ref>. 

Perhaps the biggest impact is shown in terms of the total time required for grasp synthesis. The stacked bar chart in Fig. <ref> shows the total runtime for generating 200 grasps for a given object, highlighting the individual contributions from different steps involved in the grasp synthesis process: i) query density estimation, ii) sampling and ranking and finally iii) optimising top 10 grasps (out of the 200 samples). Due to the fact that the GMM-based method requires fewer kernels to approximate the query density in Eq. <ref>, it directly affects the total runtime which is largely dominated by the number of query likelihood evaluations (Eq. <ref>).




 §.§ Conclusion & Future Work


We proposed an alternative formulation for learning generative models for grasp synthesis using parametric mixtures, such as GMMs. In our experimental setup, the GMM-based method has indicated better performance than related non-parametric approach using KDEs <cit.>. In addition to that, we extended the product of experts criterion, allowing us to accommodate task-specific constraints as needed for different deployment contexts. We gave a brief instance of that by including kinematic constraints directly in the evaluation step of the grasp synthesis pipeline on the Boris platform. Finally, we showed the GMM-based approach also offers faster execution times. This is a promising prospect that may allow us to employ the proposed method to real-time grasp synthesis as future work. Subsequent work in this direction shall be conducted on a real robot platform.






-12cm   

plain


