Skill Acquisition via Automated Multi-Coordinate Cost Balancing
Harish Ravichandar S. Reza Ahmadzadeh M. Asif Rana Sonia Chernova


Weyl points created by a three-dimensional flat band
    Feng Liu
    December 30, 2023
====================================================

empty
empty



We propose a learning framework, named Multi-Coordinate Cost Balancing (MCCB), to address the problem of acquiring point-to-point movement skills from demonstrations. MCCB encodes demonstrations simultaneously in multiple differential coordinates that specify local geometric properties. MCCB generates reproductions by solving a convex optimization problem with a multi-coordinate cost function and linear constraints on the reproductions, such as initial, target, and via points. Further, since the relative importance of each coordinate system in the cost function might be unknown for a given skill, MCCB learns optimal weighting factors that balance the cost function. We demonstrate the effectiveness of MCCB via detailed experiments conducted on one handwriting dataset and three complex skill datasets.





§ INTRODUCTION



The next generation of robots, that can operate in and adapt to unstructured and dynamic environments, must possess a diverse set of skills. However, it is implausible to pre-program robots with a library of all required skills. Learning from Demonstration (LfD) <cit.> is a paradigm that aims to equip robots with the ability to learn efficiently from demonstrations provided by humans. Existing work in trajectory-based LfD has contributed a wide range of mathematical representations that encode skills from human demonstrations and then reproduce the learned skills at runtime. Proposed representations include Spring-damper systems with forcing functions <cit.>, Gaussian Mixture Models (GMMs) <cit.>, Neural Networks (NNs) <cit.>, Gaussian Processes (GPs) <cit.>, and geometric objects <cit.>, among others. Each of these representations is used to encode the demonstrations in a predefined space or coordinate system (e.g., Cartesian coordinates). In other words, a single best coordinate system for any given skill is assumed to both exist and be known. However, as we show in this work, the assumption that a single best coordinate system exists for each task does not hold. Further, encoding in only a single coordinate system prohibits the model from capturing some of the geometric features that underlie a demonstrated skill.



In this work, we contribute a learning framework that encodes demonstrations simultaneously in multiple coordinates, and balances the relative influences of the learned models in generating reproductions. The proposed framework, named Multi-Coordinate Cost Balancing (MCCB), encodes demonstrations in three differential coordinates: Cartesian, tangent, and Laplacian (Section <ref>). Simultaneously learning in these three coordinates allows our method to capture all of the underlying geometric properties that are central to a given skill. MCCB encodes the joint density of the time index and the demonstrations in each differential coordinate frame using a separate statistical model. Thus, given any time instant, we are able to readily obtain the conditional mean and covariance in each coordinate system (Section <ref>). MCCB generates reproductions by solving an optimization problem with a blended cost function that consists of one term per coordinate. Each term penalizes deviations from the norm, weighted by the inverse of the expected variance in the corresponding coordinate system (Section <ref>). Further, we subject the optimization problem to linear constraints on the reproductions, such as initial, target, and via point constraints. Our constrained optimization problem is convex with respect to the reproduction and hence can be solved efficiently.

A major hurdle in learning a wide variety of skills, without significant parameter tweaking, is that the relative importance of each differential coordinate (or the geometric feature) in encoding a given task is unknown ahead of time. For instance, consider the problem of encoding the demonstrations illustrated in Fig. <ref>. Using any one coordinate system in isolation, even when the most suitable one is known, does not yield good reproductions (the red, brown, and green dashed lines). To alleviate this problem, MCCB preferentially weights the costs defined in each coordinate (Fig. <ref>). Importantly, MCCB learns the optimal weights directly from the demonstrations without making task-dependent assumptions. To this end, MCCB solves a meta optimization problem that aims to minimize reproduction errors (Section <ref>). As shown by the solid blue lines in Fig. <ref>, a cost function that optimally balances the costs in each coordinate yields better reproductions than any single-coordinate method.

In summary, we contribute a unified task-independent learning framework that (1) encodes demonstrations simultaneously in multiple differential coordinates, (2) defines a blended cost function that incentivizes conformance to the norm in each coordinate system while considering expected variance, and (3) learns optimal weights directly from the demonstrations to balance the relative influence of each differential coordinate in generating reproductions. Further, MCCB is compatible with and complementary to several existing LfD methods that utilize different statistical representations and coordinate systems <cit.>.






§ RELATED WORK



Learning from demonstration has attracted a lot of attention from researchers in the past few decades. While several categories of LfD methods exist <cit.>, our work falls under the category of trajectory-based LfD. In this category, demonstrations take the form of trajectories and the methods aim to synthesize trajectories that accurately reproduce the demonstrations.

Dynamical systems-based trajectory learning methods, such as <cit.>, encode demonstrations using statistical dynamical systems and generate reproductions by forward propagating the dynamics. While such deterministic methods exhibit several advantages, such as convergence guarantees and robustness to perturbations, they are restricted to learning in a single coordinate system and ignore inherent uncertainties in the demonstrations. They incentivize conformance to the norm even when demonstrations exhibit high variance.

Trajectory optimization methods, such as <cit.> and <cit.>, focus on geometric features by minimizing costs specified using predefined norms. An optimization framework proposed in <cit.> attempts to adapt multiple demonstrations to new initial and target locations by minimizing the distance between the demonstrations and the reproduction according to a learned Hilbert space norm. Indeed, learning an appropriate Hilbert space norm is related to finding an appropriate coordinate system based on the demonstrations. However, similar to the dynamical systems-based methods, the methods in <cit.> are restricted to a single predefined or learned coordinate system and do not explicitly model and utilize the inherent time-dependent variations in the demonstrations.

Probabilistic trajectory-learning methods, such as <cit.> and <cit.>, on the other hand, capture and utilize the variation observed in the demonstrations. However, these methods are also restricted to encoding demonstrations in a single predefined coordinate system that is assumed to be known.

Our design of the costs in each differential coordinate is inspired by the minimal intervention principle <cit.> that takes variance into account. While the approach in <cit.> does encode demonstrations in different frames of references, all the frames are restricted to Cartesian coordinates or orientation space. Furthermore, all the relevant frames for a given task are also expected to be provided by the user.

The motion planning framework in <cit.>, complementary to our approach, utilizes a blended cost function, the construction of which is guided by probability distributions learned from the demonstrations. This framework incentivizes factors such as smoothness, manipulability, and obstacle avoidance, but is restricted to the Cartesian coordinate system. MCCB, on the other hand, encodes demonstrations in multiple differential coordinates and learns to optimally balance their relative influences, but does not consider factors such as manipulability and obstacle avoidance.

Differential coordinates have been extensively used in the computer graphics community <cit.>. Prior work in trajectory learning that incorporates differential coordinates includes the Laplacian trajectory editing (LTE) algorithm <cit.>. Using Laplacian coordinates, the  LTE algorithm adapts a single demonstration to new initial, target, and via points while preserving the shape. However, the LTE algorithm does not reason about the relative importances of multiple coordinates.




§ METHODOLOGY


The section describes the technical details of MCCB and its work flow as illustrated in Fig. <ref>.



 §.§ Differential Coordinate Transformations
 
In this section, we define the differential coordinates and their corresponding transformations used in MCCB.

Cartesian: Let a discrete finite-length trajectory in n-dimensional Cartesian coordinates be denoted by X = [x(1)  x(2) ⋯ x(T)]^⊤∈ℝ^T × n and let x(t) ∈ℝ^n denote a discrete sample at time index t. This trajectory can be represented using a graph 𝒢=(𝒱,ℰ) where 𝒱 is the set of vertices representing the samples in the trajectory and ℰ is the set of edges that represent the connections between the samples in the trajectory. The neighborhood 𝒩_t of each vertex 𝒱_t is defined by the set of adjacent vertices 𝒱_t'. In the case of discrete-time trajectories, the edges between any given vertex and its two neighbors are assumed to carry unit weights, while all other edges carry zero weights.

Laplacian: It is known that the discrete Laplace-Beltrami operator for the trajectory X provides the Laplacian coordinate δ(t) as
δ(t) ≜∑_t' ∈𝒩_t1/∑_t' ∈𝒩_t 1 (x(t) - x(t')) <cit.>.
Note that the above relationship can be written as a linear differential operator in matrix form

    Δ = LX

where Δ = [δ(1)  δ(2) ⋯δ(T)]^⊤∈ℝ^T × n is the trajectory in the Laplacian coordinates, and L∈ℝ^T × T, called the graph Laplacian, is given by

    L = [[    1   -1    0    …    …    0; -0.5    1 -0.5    0    …    0;    0 -0.5    1 -0.5    …    0;    ⋮         ⋱    ⋱    ⋱    ⋮;    0    …    0 -0.5    1 -0.5;    0    …    …    0   -1    1 ]]

As pointed out in <cit.>, the Laplacian coordinates have meaningful geometric interpretations. Specifically, the Laplacian coordinates can be seen as the discrete approximations of the derivative of the unit tangent vectors of an arc-length parametrized continuous trajectory. In other words, the Laplacian coordinates measure the deviation of each sample from the centroid of its neighbors.

Tangent: While the Laplacian coordinates are discrete approximations of second order differential transformations, a discrete approximation of the first differential transformation is possible. Consider such a first order transformation using first order finite differences defined as
γ(t) ≜ (x(t+1) - x(t)),
where γ(t) is called the tangent coordinate. The matrix form of the above relationship results in a linear differential operator given by

    Γ = GX

where Γ = [γ(1)  γ(2) ⋯γ(T)]^⊤∈ℝ^T × n is the trajectory in the tangent coordinates and G∈ℝ^T × T, called the graph incidence matrix, is given by

    G = [[ -1  1  0  …  …  0;  0 -1  1  0  …  0;  0  0 -1  1  …  0;  ⋮     ⋱  ⋱  ⋱  ⋮;  0  …  0  0 -1  1;  0  …  …  0  0 -1 ]]

Similar to the Laplacian coordinates, the tangent coordinates have geometric interpretations. Specifically, the tangent coordinates can be seen as discrete approximations of the un-normalized tangent vectors of an arc-length parametrized continuous trajectory, i.e., the tangent coordinates measure the local direction of motion at each sample of the trajectory.

In our work, we assume that a set of N demonstrations in the Cartesian coordinates are available. Let the jth demonstration be denoted by X_d^j = [x_d^j(1)  x_d^j(2) ⋯ x_d^j(T)]^⊤∈ℝ^T × n. Note that if the raw demonstrations are of varying duration in time, we perform time alignment using dynamic time warping. MCCB transforms each obtained demonstration X_d^j into a trajectory in the tangent coordinates (denoted by Γ_d^j) and a trajectory in Laplacian coordinates (denoted by Δ_d^j) using (<ref>) and (<ref>), respectively.




 §.§ Encoding in Multiple Differential Coordinates


This section defines the costs associated with each coordinate. With the demonstrations available in all three differential coordinates, we employ three independent Gaussian mixture models (GMMs)[MCCB does not rely on the use of GMMs and any statistical representation that can provide the conditional estimates will suffice.] to approximate the joint probability densities of time and the samples in each coordinate system.

The GMM associated with the Cartesian coordinates attempts to approximate the joint density of t and x using a finite number of Gaussian basis functions as follows
𝒫(t,x;θ_C) = ∑_k=1^K_C𝒫(k) 𝒫(t,x|k),
where K_C is the number of Gaussian basis functions, 𝒫(k) = π_C^k is the prior associated with the kth basis function, θ_C = {μ_C^1 ⋯μ_C^K_C, Σ_C^1 ⋯Σ_C^K_C, π_C^1 ⋯π_C^K_C} is the set of parameters of the GMM, and 𝒫(t,x|k) is the conditional probability density given by
𝒫(t,x|k) ∼𝒩(
[ t; x ];
μ_C^k,Σ_C^k),
where μ_C^k = [ μ_t^k; μ_x^k ] is the mean and Σ_C^k = [   Σ_t^k Σ_t,x^k; Σ_x,t^k   Σ_x^k ] is the covariance matrix of the kth Gaussian basis function.

We learn the parameters θ_C of the model using the Expectation-Maximization algorithm based on the demonstrations {X_d^j}_j=1^N. Given the learned model and a time instant, the expected value of the conditional density 𝒫(x|t) is given by Gaussian mixture regression (GMR) <cit.> as follows


    x̂(t) = 𝔼[x|t] = ∑_k=1^K_C h_C^k(t) (A_C^k t + b_C^k)

where h_C^k(t) = 𝒫(k) 𝒫(t|k)/∑_i=1^K_C𝒫(i) 𝒫(t|i), A_C^k = Σ_x,t^k (Σ_t^k)^-1, b^k = μ_x^k +  (t - μ_t^k), and the conditional covariance is given by

    Σ̂_x(t) = Var[x|t] = ∑_k=1^K_Ch_C^k^2  (Σ_x^k - Σ_x,t^k(Σ_t^k)^-1Σ_t,x)


Similar to the GMM learned in the Cartesian coordinates, we learn a second GMM in the tangent coordinates based on the demonstrations {Γ_d^j}_j=1^N, and a third GMM in the Laplacian coordinates based on the demonstrations {Δ_d^j}_j=1^N. The expected values of the conditional densities 𝒫(γ|t) and 𝒫(δ|t) are given by

    γ̂(t) =    𝔼[γ|t] = ∑_k=1^K_G h_G^k(t) (A_G^k t + b_G^k) 
    δ̂(t) =    𝔼[δ|t] = ∑_k=1^K_L h_L^k(t) (A_L^k t + b_L^k)

and the corresponding conditional expectations are given by

    Σ̂_γ(t) =   Var[γ|t] = ∑_k=1^K_G (h_G^k)^2  (Σ_γ^k -Σ_γ,t^k(Σ_t^k)^-1Σ_t,γ) 
    Σ̂_δ(t) =   Var[δ|t] = ∑_k=1^K_L (h_L^k)^2  (Σ_δ^k -Σ_δ,t^k(Σ_t^k)^-1Σ_t,δ)

where the variables in (<ref>)-(<ref>) with subscripts G and L correspond to the tangent and Laplacian coordinates, respectively, and are defined similarly to the ones in (<ref>)-(<ref>).




 §.§ Imitation via Optimization

In this section, we explain the design of our multi-coordinate cost function. MCCB generates reproductions by solving a constrained optimization problem given by

    X_r =    min_X w_C J_C(X) + w_G J_G(X) 
                    + w_L J_L(X) 
        s.t.     P_x X = X^*

where X_r ∈ℝ^T × n is the reproduction, w_C, w_G, w_L ∈ℝ^+ are positive weights; J_C, J_G, J_L: ℝ^T × n→ℝ^+ are cost functions in the Cartesian, tangent, and Laplacian coordinates, respectively; P_x ∈ℝ^m × T and X^* ∈ℝ^m × n define m ∈ℤ^+ linear constraints on X_r. In practice, m<<n and we use the linear constraints to enforce constraints on initial, target, and via points.

We define the cost function in each coordinate system as follows

    J_C(X) =     (X(:) - X̂(:))^⊤ (Σ̂_X)^-1  (X(:) - X̂(:)) 
    
    J_G(X) =     (Γ(:) - Γ̂(:))^⊤ (Σ̂_Γ)^-1  (Γ(:) - Γ̂(:)) 
    
    J_L(X) =     (Δ(:) - Δ̂(:))^⊤ (Σ̂_Δ)^-1  (Δ(:) - Δ̂(:))

where Σ̂_X, Σ̂_Γ, Σ̂_Δ∈ℝ^nT × nT denote the block diagonal matrices formed with the conditional covariances Σ̂_x (t), Σ̂_γ (t), and Σ̂_δ (t), respectively, for all values of t. Further, the notation (:) denotes vectorization - for instance, X(:), X̂(:) ∈ℝ^nT denote the vectorized trajectories formed by vertically stacking x(t) and x̂(t) for all values of t, respectively. Note that we construct the trajectories Γ and Δ in (<ref>) and (<ref>) from X via the linear operators defined in (<ref>) and (<ref>), respectively. MCCB penalizes deviations from the conditional mean in each coordinate system. However, deviations are penalized less (more) severely if high (low) variance is observed in the demonstrations at any given time.




 §.§ Automated Cost Balancing

In order to obtain reproductions that successfully imitate demonstrations of a wide variety of skills, the weights w_C, w_G, and w_L have to be chosen with care. Indeed, they preferentially weight the costs defined in each differential coordinate and thereby manipulate the relative incentive for successful imitation in each coordinate system.

We learn these weights directly from the available demonstrations. Note that, for known weights, the constrained optimization problem in (<ref>) is convex in X. We estimate the weights in the following form

    ŵ_C = α_C/β_C;   ŵ_G = α_G/β_G;   ŵ_L = α_L/β_L

where β_C, β_G, β_L ∈ ( 0, 1 ], such that ∑_i β_i = 1, are positive scaling factors used to correct for inherent differences in the magnitudes of the costs, and α_C, α_G, α_L ∈ [ 0, 1 ], such that ∑_i α_i = 1, are positive weights used to preferentially weight the cost defined in each coordinate system. MCCB estimates the scaling factors β_i's as follows

    β_i = ∑_j=1^N J_i(X_d^j)/∑_l ∑_j=1^N J_l(X_d^j),   ∀ i,l = {C,G,L}


With the scaling factors compensating the inherent scale difference in the costs, we compute the preferential weighting factors α_i's that minimize reproduction error. To this end, we formulate the following meta optimization problem

    {α_C, α_G, α_L} =    min_α_C, α_G, α_L∑_j=1^N SSE(X_r^j,X_d^j) 
       s.t.∑_i α_i = 1, ∀ i = {C,G,L}

where SSE(·) denotes the sum of squared errors computed over time, and X_r^j is the solution to the following optimization problem

    X_r^j =    min_X (α_C/β_C) J_C(X) + (α_G/β_G) J_G(X)  
                    + (α_L/β_L) J_L(X) 
        s.t.     P_x X = X_j^*

where P_x X = X_j^* denotes specific linear constraints pertaining to the demonstration X_d^j, such as initial, target, and via points. Solving the above meta-optimization problem results in the preferential weights α_i's that minimize reproduction errors of the solutions generated by the original constrained optimization problem in (<ref>)-(<ref>).




§ EXPERIMENTAL EVALUATION


This section describes the design and discusses the results of four experiments conducted to evaluate MCCB. In each experiment, we compared the performances of the following approaches:


  * Cartesian-coordinates: w_C = 1, w_G=0, w_L=0

  * Tangent-coordinates: w_C = 0, w_G=1, w_L=0

  * Laplacian-coordinates: w_C = 0, w_G=0, w_L=1

  * Uniform weighting: w_C = 1/3, w_G=1/3, w_L=1/3

  * MCCB: w_C = ŵ_̂Ĉ, w_G=ŵ_̂Ĝ, w_L=ŵ_̂L̂




We measured the performance of each approach by the following geometric and kinematic metrics: Swept Error Area (SEA) <cit.>, Sum of Squared Errors (SSE), Dynamic Time Warping Distance (DTWD), and Frechet Distance (FD) <cit.>. These metrics allow us to evaluate different aspects of each method's performance. The SEA and SSE metrics penalize both spatial and temporal misalignment, and thus evaluate kinematic performance. On the other hand, the DTWD and FD metrics penalize spatial misalignment while disregarding time misalignment, and thus evaluate geometric performance. Further, the SEA, SSE, and DTWD metrics evaluate aggregate performance by summing over or averaging across all the samples of each reproduction. The FD metric, on the other hand, computes the shortest possible cord length required to connect the demonstration and the reproduction in space while allowing time re-parametrization of either trajectory, and thus measures maximal deviation in space. Note that the SEA metric is restricted to 2-dimensional data, so we only report it for one of our experiments.

In all the experiments, we used the position constraints in (<ref>) to enforce both initial and end point constraints uniformly across all the methods being compared. Further, we uniformly set the number of Gaussian basis functions to five across all the coordinates and all the experiments.




 §.§ Handwriting Skill

This experiment evaluates MCCB on the publicly available LASA human handwriting library <cit.>, that consists of handwriting motions collected from pen input using a Tablet PC. The library contains a total of 25 handwriting motions, each with 7 demonstrations. 







Fig. <ref> shows that MCCB yields reproductions that are qualitatively similar to the demonstrations while satisfying the end-point constraints across all motions. As shown in Fig. <ref>, quantitative analysis indicates that MCCB (α̅_C = 0.1814, α̅_G = 0.4958, α̅_L = 0.3228)[Weighting factors averaged over all 25 skills in the LASA dataset] and three of the four baselines performed comparably with respect to the SEA, FD, SSE, and DTWD metrics, while the Cartesian baseline performed poorly in comparison. This is consistent with the fact that the demonstrations within the LASA dataset emphasize strong similarities in shape.




 §.§ Picking Skill


The second experiment evaluates the performance of MCCB in a picking task (Fig. <ref>). The data consists of six kinesthetic demonstrations, each a 3-dimensional robot end-effector position trajectory recorded as a human guided the robot in picking up two magnets atop two blocks. We enforced two via-point constraints (one at each picking point) in addition to the end-point constraints. 



As shown in Fig. <ref>(a), MCCB generated reproductions that are qualitatively similar to the demonstrations while satisfying all the position constraints. Quantitative evaluations reveal that learning in tangent coordinates yielded better reproductions than learning in Cartesian and Laplacian coordinates (Fig. <ref>). This was expected since the demonstrations of this task, much like the LASA dataset, emphasize shape similarity. Further, MCCB (α_C = 0.2362, α_G = 0.5451, α_L = 0.2187) yielded the best performance, with respect to all three metrics. In fact, uniform weighting yielded poorer results, with respect to all three metrics, than when considering only the tangent coordinates. The results of this experiment show that while multi-coordinate methods can yield strong performance, it is critical that we balance the weights appropriately.






 §.§ Pressing Skill

In this experiment, we evaluated MCCB's ability to learn pressing skills (Fig. <ref>). The data consists of six kinesthetic demonstrations, each a 3-dimensional robot end-effector position trajectory recorded as a human guided the robot in pressing two cylindrical pegs into their respective holes. 



As shown in Fig. <ref>(b), MCCB successfully reproduced the demonstrations. Note that MCCB is capable of automatically capturing and reproducing the consistencies across the demonstrations in certain regions without any position constraints. Fig. <ref> illustrates the performance of MCCB and the baselines with respect to three different metrics. Learning in Cartesian coordinates resulted in the better performance compared to learning in tangent and Laplacian coordinates. Quantitative evaluations further demonstrate that MCCB (α_C = 0.6735, α_G = 0.2034, α_L = 0.1231) consistently yielded the best performance with respect to all three metrics. The results of this experiment, in light of the results in Section <ref>, suggest that the relative importance of each of the differential coordinates vary across different skills.




 §.§ Pushing Skill



The final experiment evaluates the performance of MCCB in a pushing task  (Fig. <ref>). The data consists of six kinesthetic demonstrations, each a 3-dimensional robot end-effector position trajectory recorded as a human guided the robot in sliding closed the lid of a wooden box. 



As shown in Fig. <ref>(c), MCCB successfully generated reproductions that are similar to the demonstrations. As evidenced by quantitative evaluations in Fig. <ref>, encoding demonstrations in the Laplacian coordinates yielded better performance, with respect to all three metrics, when compared to learning only in either of the other two coordinates, while, MCCB (α_C = 0.0123, α_G = 0.045, α_L = 0.9427) consistently outperformed all the other approaches. Note that learning in the Laplacian coordinates alone resulted in better performance than uniformly weighting of all the coordinates. These results are consistent with the results from the previous sections and indicate that MCCB yields consistently good performance. The results are summarized in Table <ref>.






§ DISCUSSION AND CONCLUSION




We introduced MCCB, a learning framework for encoding demonstrations in multiple differential coordinates, and automated balancing of costs defined in those coordinates.  As shown in Table <ref>, we demonstrated that the relative effectiveness of each coordinate system is not consistent across a variety of tasks since any given skill might be better suited for learning in one (or more) coordinate system(s). Furthermore, uniform weighting of costs in different coordinates does not consistently yield the best results across different skills. Indeed, uniform weighting, in some cases, yielded poorer performances compared to when only one coordinate system was used. On the other hand, MCCB learned to balance the costs and consistently yielded the best performance. Since the weights are learned directly from the demonstrations, MCCB makes no task-specific assumptions and does not require tedious parameter tuning.  Note that although we used GMMs as the base representation in this work, MCCB is agnostic to the statistical model used to encode the demonstrations in each coordinate system, and thus can be combined with other techniques, such as <cit.>.  Furthermore, MCCB can be extended to include more coordinate systems that capture additional trajectory features.


                                  
                                  
                                  
                                  
                                  








§ ACKNOWLEDGMENT


This research is supported in part by NSF NRI 1637758.

IEEEtran



