Method and system for robot manipulation planning

Abstract

A method for planning a manipulation task of an agent, particularly a robot. The method includes: learning a number of manipulation skills wherein a symbolic abstraction of the respective manipulation skill is generated; determining a concatenated sequence of manipulation skills selected from the number of learned manipulation skills based on their symbolic abstraction so that a given goal specification indicating a given complex manipulation task is satisfied; and executing the sequence of manipulation skills.

Claims

1. A computer-implemented method for planning a manipulation task of a robot, comprising the following steps: learning a number of manipulation skills and generating a symbolic abstraction of each of the manipulation skills, by which a respective statistic model of a transition to a respective final state from a respective current state is determined for each of the learned manipulation skills; determining a concatenated sequence of a subset of the manipulation skills selected from the number of learned manipulation skills based on their symbolic abstractions so that a given goal specification indicating a given complex manipulation task is satisfied, wherein: the determining of the concatenated sequence includes generating a complete model corresponding to a combination, in the sequence, of all of the subset of the manipulation skills; and the generating of the complete model is performed by computing, for each of a plurality of pairwise combinations of individual manipulation skills of the subset, a respective probability of a transition from a first one of the individual manipulation skills of the respective pairwise combination to a second one of the individual manipulation skills of the respective pairwise combination according to a divergence of emission probabilities between an end state of the first one of the individual manipulation skills of the respective pairwise combination and an initial state of the second one of the individual manipulation skills of the respective pairwise combination; and executing the sequence of manipulation skills.

2. The method according to claim 1, wherein: the statistical model is a Task Parameterized Hidden Semi-Markov model (TP-HSMM); and the learning of the number of manipulation skills includes, for each of the number of manipulation skills: demonstrating and recording a plurality of manipulation trajectories; determining a respective instance of the TP-HSMM depending on the plurality of manipulation trajectories of the respective manipulation skill; and determining a respective one of the symbolic abstractions.

3. The method according to claim 2, wherein the generating of the symbolic abstractions of the manipulation skills includes constructing a Planning Domain Definition Language (PDDL) model in which objects, the initial states, and a goal specification define problem instances, and in which predicates and actions define a domain of a given manipulation, wherein the symbolic abstractions of the manipulation skills use a classical PDDL planning language.

4. The method according to claim 2, where the determining of the concatenated sequence of manipulation skills is performed such that a probability of achieving the given goal specification is maximized, wherein a PDDL planning step is used to find a sequence of actions to fulfill the given goal specification, starting from a given one of the initial states.

5. The method according to claim 2, where the transition probabilities are determined using Expectation-Maximization.

6. The method according to claim 2, wherein the symbolic abstractions are determined by mapping low-variance geometric relations of segments of the manipulation trajectories into a set of predicates.

7. The method according to claim 2, wherein the determining of the concatenated sequence of manipulation skills includes optimizing a goal of minimization of a total length of a combined trajectory of the concatenated sequence.

8. The method according to claim 7, wherein the determining of the concatenated sequence of manipulation skills includes selectively reproducing one or more of the manipulation skills of the sequence of manipulation skills so as to maximize a probability of satisfying the given goal specification.

9. The method according to claim 7, wherein the determining of the concatenated sequence of manipulation skills further includes searching a most-likely complete state sequence between initial and goal states of the complex manipulation task as a whole using a Viterbi algorithm.

10. The method according to claim 7, wherein the determining of the concatenated sequence of manipulation skills further includes searching a most-likely complete state sequence between initial and goal states of the manipulation task as a whole using a Viterbi algorithm that includes missing observations and duration probabilities.

11. The method according to claim 1, wherein an individual one of the manipulation skills of the subset is included multiple times within the concatenated sequence.

12. A device for planning a manipulation task of a robot, the device comprising a processor, the processor configured to: learn a number of manipulation skills, thereby generating a symbolic abstraction of each of the manipulation skills and by which a respective statistic model of a transition to a respective final state from a respective current state is determined for each of the learned manipulation skills; determine a concatenated sequence of a subset of the manipulation skills selected from the number of learned manipulation skills based on their symbolic abstractions so that a given goal specification indicating a complex manipulation task is satisfied; and instruct execution of the sequence of manipulation skills; wherein: the determination of the concatenated sequence includes generating a complete model corresponding to a combination, in the sequence, of all of the subset of the manipulation skills; and the generating of the complete model is performed by computing, for each of a plurality of pairwise combinations of individual manipulation skills of the subset, a respective probability of a transition from a first one of the individual manipulation skills of the respective pairwise combination to a second one of the individual manipulation skills of the respective pairwise combination according to a divergence of emission probabilities between an end state of the first one of the individual manipulation skills of the respective pairwise combination and an initial state of the second one of the individual manipulation skills of the respective pairwise combination.

13. A non-transitory machine-readable storage medium on which is stored a computer program that is executable by a computer and that, when executed by the computer, causes the computer to perform a method for planning a manipulation task of a robot, the method comprising the following steps: learning a number of manipulation skills and generating a symbolic abstraction of each of the manipulation skills, by which a respective statistic model of a transition to a respective final state from a respective current state is determined for each of the learned manipulation skills; determining a concatenated sequence of a subset of the manipulation skills selected from the number of learned manipulation skills based on their symbolic abstractions so that a given goal specification indicating a given complex manipulation task is satisfied, wherein: the determining of the concatenated sequence includes generating a complete model corresponding to a combination, in the sequence, of all of the subset of the manipulation skills; and the generating of the complete model is performed by computing, for each of a plurality of pairwise combinations of individual manipulation skills of the subset, a respective probability of a transition from a first one of the individual manipulation skills of the respective pairwise combination to a second one of the individual manipulation skills of the respective pairwise combination according to a divergence of emission probabilities between an end state of the first one of the individual manipulation skills of the respective pairwise combination and an initial state of the second one of the individual manipulation skills of the respective pairwise combination; and executing the sequence of manipulation skills.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

(1) Example embodiments of the present invention are described in more detail in conjunction with the figures.

(2) FIG. 1 schematically shows a robot arm.

(3) FIG. 2 shows a flowchart illustrating the method or manipulation of an object by sequencing of manipulation skills.

(4) FIG. 3 shows illustrations for HSMM states of a learned manipulation skill wherein demonstration trajectories and transition probabilities are illustrated.

DETAILED DESCRIPTION OF EXAMPLE EMBODIMENTS

(5) FIG. 1 shows a system (agent) 1 including a controllable robot arm 2 as an example of an object manipulator. The robot arm 2 is a multi-DoF robotic arm with several links 21 and an end-effector 22 which allows a state x∈.sup.3×S.sup.3×.sup.1 (describing the Cartesian position, orientation and gripper state in a global coordinate system (frame)), that operates within a static or dynamic and known workspace. Also, within the reach of the robot arm 2, there are objects of interest denoted by O={o.sub.1, o.sub.1, . . . , o.sub.J}.

(6) Within this setup, a human user can perform several kinesthetic demonstrations on the arm to manipulate one or several objects for certain manipulation skills. Denote by A={a.sub.1, a.sub.1, . . . , a.sub.H} the set of demonstrated skills. Moreover, for manipulation skill a.sub.h∈A, the set of objects involved is given by O.sub.a.sub.h and the set of available demonstrations is denoted by D.sub.a.sub.h.

(7) The robot arm 2 is controlled by means of a control unit 3 which may actuate actuators to move the robot arm 2 and activate the end effector 22. Sensors may be provided at the robot arm 2 or at the robot workspace to record the state of objects in the robot workspace. Furthermore, the control unit 3 is configured to record movements made with the robot arm 2 and to obtain information about objects in the workspace from the sensors and further to perform a task planning process as described below. The control unit 3 has a processing unit where the algorithm as described below is implemented in hardware and/or software.

(8) All demonstrations are described by the structure of TP-GMM (task-parametrized-Gaussian Mixture Models). The basic idea is to fit a prescribed skill model such as GMMs to multiple demonstrations. GMMs are described, e.g., in S. Niekum et al. “Learning grounded finite-state representations from unstructured demonstrations”, The International Journal of Robotics Research, 34(2), pages 131-157, 2015. For a number M of given demonstrations (trajectory measurement results), each of which contains T.sub.m data points for a dataset, N=M*T.sub.m total observations ξ={ξ.sub.t}.sub.t=1.sup.N exists, where ξ.sub.t∈.sup.d for sake of clarity. Also, it is assumed the same demonstrations are recorded from the perspective of P different coordinate systems (given by the task parameters such as objects of interest). One common way to obtain such data is to transform the demonstrations from global frame (global coordinate system) to frame p by ξ.sub.t.sup.(p)=A.sub.t.sup.(p).sup.−1(ξ.sub.t−b.sub.t.sup.(p)), where {(b.sub.t.sup.(p),A.sub.t.sup.(p)}.sub.p=1.sup.P is the translation and rotation of frame p with respect to the given global frame at time t. Then, a TP-GMM (task-parametrized-Gaussian Mixture Model) is described by the parameters {π.sub.k,{μ.sub.k.sup.(p),Σ.sub.k.sup.(p)}.sub.p=1.sup.P}.sub.k=1.sup.K where K represents the number of Gaussian components in the mixture model, π.sub.k is the prior probability of each component, and {μ.sub.k.sup.(p),Σ.sub.k.sup.(p)}.sub.p=1.sup.P are the parameters of the k-th component within frame p. K may be manually assigned. However, there are approaches to automatically set K.

(9) Differently from standard GMM learning, the mixture model above cannot be learned independently for each frame p. Indeed, the mixing coefficients π.sub.k are shared by all frames p and the k-th component in frame p must map to the corresponding k-th component in the global frame. For example, Expectation-Maximization (EM) is a well-established method to learn such models. In general, an expectation-maximization (EM) algorithm is an iterative method to find maximum likelihood or maximum a posteriori (MAP) estimates of parameters in a statistical model, which depends on unobserved latent variables.

(10) Once learned, the TP-GMM can be used during execution to reproduce a trajectory for the learned skill. Namely, given the observed frames {b.sub.t.sup.(p),A.sub.t.sup.(p)}.sub.p=1.sup.P, the learned TP-GMM is converted into one single GMM with parameters {π.sub.k,μ.sub.t.sup.(p),Σ.sub.t.sup.(p)}.sub.k=1.sup.K, by multiplying the affine-transformed Gaussian components across different frames, as follows

(11) ${\left(\underset{t,k}{\overset{^}{.Math.}}\right)}^{-1}=\underset{p=1}{\overset{P}{.Math.}}{\left({\widehat{.Math.}}_{t,k}^{\left(p\right)}\right)}^{-1},{\hat{\mu }}_{t,k}=\underset{t,k}{\overset{^}{.Math.}}\underset{p=1}{\overset{P}{.Math.}}{\left({\widehat{.Math.}}_{t,k}^{\left(p\right)}\right)}^{-1}{\hat{\mu }}_{t,k}^{\left(p\right)}$
where the parameters of the updated Gaussian at each frame p are computed as
{circumflex over (μ)}.sub.t,k.sup.(p)=A.sub.t.sup.(p)μ.sub.k.sup.(p)+b.sub.t.sup.(p) and {circumflex over (Σ)}.sub.t,k.sup.(p)=A.sub.t.sup.(p)μ.sub.k.sup.(p)+b.sub.t.sup.(p)

(12) Hidden semi-Markov Models (HSMMs) have been successfully applied, in combination with TP-GMMs, for robot skill encoding to learn spatio-temporal features of the demonstrations, such as manipulation trajectories of a robot or trajectories of a movable agent.

(13) Hidden semi-Markov Models (HSMMs) extend standard hidden Markov Models (HMMs) by embedding temporal information of the underlying stochastic process. That is, while in HMM the underlying hidden process is assumed to be Markov, i.e., the probability of transitioning to the next state depends only on the current state, in HSMM the state process is assumed semi-Markov. This means that a transition to the next state depends on the current state as well as on the elapsed time since the state was entered.

(14) More specifically, a task parametrized HSMM model consists of the following parameters
M.sub.θ={{a.sub.kh}.sub.h=1.sup.K,(μ.sub.k.sup.D,σ.sub.k.sup.D),π.sub.k,{μ.sub.k.sup.(p),Σ.sub.k.sup.(p)}.sub.p=1.sup.P}.sub.k=1.sup.K
where a.sub.kh is the transition probability from state k to h; (μ.sub.k.sup.D,σ.sub.k.sup.D) describes the Gaussian distributions for the duration of state k, i.e., the probability of staying in state k for a certain number of consecutive steps; {π.sub.k,{μ.sub.k.sup.(p),Σ.sub.k.sup.(p)}.sub.p=1.sup.P}.sub.k=1.sup.K equals the TP-GMM introduced earlier and, for each k, describe the emission probability, i.e., the probability of observation, corresponding to state k. In an HSMM the number of states correspond to the number of Gaussian components in the “attached” TP-GMM. In general, HSMM states are Gaussian distributions, which means that its observation probability distribution is represented as a classical GMM. To render HSMM to be object-centric, the observation probabilities can be parametrized as it is done in TP-GMM to obtain a TP-HSMM.

(15) Given a certain sequence of observed data points , the associated sequence of states in is given by s.sub.t=s.sub.1, s.sub.2 . . . s.sub.t. The probability of data point ξ.sub.t belonging to state k (i.e., s.sub.t=k) is given by the forward variable a.sub.t(k)=p(s.sub.t=k,{):

(16) $a_t\left(k\right)=\underset{\tau -1}{\overset{t-1}{.Math.}}\underset{h-1}{\overset{K}{.Math.}}{a}_{t-\tau }\left(h\right)a_{\mathrm{hk}}N\left(\tau |{\mu }_k^D,{\sigma }_k^D\right)o_{\tau }^t$
where o.sub.τ.sup.t=N() is the emission probability, where ({circumflex over (μ)}.sub.l,k,{circumflex over (Σ)}.sub.l,k) are derived from ({circumflex over (Σ)}.sub.t,k).sup.−1=Σ.sub.p=1.sup.P({circumflex over (Σ)}.sub.t,k.sup.(p)).sup.−1,{circumflex over (μ)}.sub.t,k={circumflex over (Σ)}.sub.t,kΣ.sub.p=1.sup.P({circumflex over (Σ)}.sub.t,k.sup.(p)).sup.−1{circumflex over (μ)}.sub.t,k.sup.(p) as shown above. Furthermore, the same forward variable can also be used during reproduction to predict future steps until T.sub.m. In this case however, since future observations are not available, only transition and duration information are used, i.e., by setting N()=1 for all k and >t in a.sub.t(k)=Σ.sub.τ-1.sup.t-1Σ.sub.h-1.sup.Ka.sub.t-τ(h)a.sub.hkN(τ|μ.sub.k.sup.D,σ.sub.k.sup.D)o.sub.τ.sup.t. At last, the sequence of the most-likely states s*.sub.T.sub.m=s*.sub.1, s*.sub.2 . . . s*.sub.T.sub.m is determined by choosing s*.sub.t=argmax.sub.ka.sub.t(k),∀1≤t≤T.sub.m.

(17) All demonstrations are recorded from multiple frames. Normally, these frames are closely attached to the objects in O.sub.a.sub.h. For example, the skill “insert the peg in the cylinder” involves the objects “peg” and “cylinder”, and the associated demonstrations are recorded from both the robot, the “peg” and the “cylinder” frames, respectively.

(18) In addition, consider a set of pre-defined predicates, denoted by B={b.sub.i, b.sub.2, . . . , b.sub.L}, representing possible geometric relations among the objects of interest. Here, predicates b∈B are abstracted as Boolean functions taking as inputs the status of several objects while outputting whether the associated geometric relation holds or not. For instance, grasp:=O.fwdarw.B indicates whether an object is grasped by the robot arm; within: O×O.fwdarw.B indicates whether an object is inside another object; and onTop: O×O.fwdarw.B indicates whether an object is on the top of another object. Note that these predicates are not bound to specific manipulation skills but rather shared among them. Usually, such predicate functions can be easily validated for the robot arm states and the object states (e.g., position and orientations).

(19) Finally, a goal specification G is given as a propositional logic expression over the predicates B, i.e., via nested conjunction, disjunction and negation operators. In general, the goal specification G represents the desired configuration of the arm and the objects, assumed to be feasible. As an example, one specification could be “within(peg,cylinder)ΛonTop(cylinder, box)”, i.e., “the peg should be inside the cylinder and the cylinder should be on top of the box”.

(20) In the following, a problem can be defined as follows: Given a set of demonstrations D for skills A and the goal G, the objective is a) to learn a TP-HSMM model M.sub.θ of the form
M.sub.θ={{a.sub.kh}.sub.h=1.sup.K,(μ.sub.k.sup.D,σ.sub.k.sup.D),π.sub.k,{μ.sub.k.sup.(p),Σ.sub.k.sup.(p)}.sub.p=1.sup.P}.sub.k=1.sup.K,
for each demonstrated skill a.sub.h and reproduce the skill for a given final configuration G. For the reproduction of the skill the sequence of states obtained through Viterbi algorithm can be used. b) to construct a PDDL model P of the form
P=(P.sub.D,P.sub.P)
where Objects, Initial State and Goal Specification define a problem instance P.sub.P, while Predicates and Actions define the domain of a problem P.sub.D; c) to derive and subsequently execute the sequence of manipulation skills, such that the probability of achieving the given goal specification G is maximized. Once the domain and problem files are specified, a PDDL (Planning Domain Definition Language.) planner has to find a sequence of actions to fulfill the given goal specification, starting from the initial state.

(21) The PDDL model P includes a domain for the demonstrated skills and a problem instance given the goal specification G.

(22) The Planning Domain Definition Language (PDDL) is the standard classic planning language. Formally, the language consists of the following key ingredients: Objects, everything of interest in the world; Predicates, object properties and relations; Initial States, set of grounded predicates as the initial states; A goal Specification, the goal states; and Actions, how predicates are changed by an action and also the preconditions on the actions.

(23) In the example embodiment described herein, motion planning is performed at the end-effector/gripper trajectory level. This means it is assumed that a low-level motion controller is used to track the desired trajectory.

(24) The example method for planning a manipulation task is described in detail with respect to the flowchart of FIG. 2.

(25) Firstly, a TP-HSMM model M.sub.θ is to be learned for each demonstrated skill a.sub.h and reproduce the skill for a given final configuration G.

(26) One demonstrated skill a.sub.h∈A is considered. As described above, the set of available demonstrations, recorded in P frames, is given by D.sub.a.sub.h={ξ.sub.t}.sub.t=1.sup.N. Furthermore, the set of objects involved with skill a.sub.h is given by O.sub.a.sub.h. The P frames are directly attached to the objects in O.sub.a.sub.h.

(27) Given a properly chosen number of components K which correspond to the TP-HSMM states, which are the Gaussian components representing the observation probability distributions, the TP-HSMM model M.sub.a.sub.h abstracting the spatio-temporal features of trajectories related to skill a.sub.h, can be learned in step S1 using, e.g., an EM-like algorithm. This is beneficial as only one model for the general skill is constructed. FIG. 3 shows an example of an HSMM for “pick the peg” skill that contains 10 demonstrations for “pick from top” and “pick from side”. The learned HSMM model in the global frame has a single initial HSMM state from which two branches encode the two different instantiations of the same “pick” skill. FIG. 3 shows illustrations for HSMM states of a learned skill wherein demonstration trajectories in 2D(left) and transition probabilities between associated states are shown.

(28) A final goal configuration G is provided in step S2 which can be translated into the final state of the end effector 22 x.sub.G∈.sup.3×S.sup.3×.sup.1. This configuration can be imposed as the desired final observation of the reproduction, i.e., ξ.sub.T.sub.m≡x.sub.G. Similarly, the initial configuration of the end-effector 22 can be imposed as the initial observation, i.e., ξ.sub.0≡x.sub.0. Following, the most likely sequence s*.sub.T.sub.m given only by ξ.sub.0 and ξ.sub.T.sub.m.

(29) The forward variable of formula

(30) $a_t\left(k\right)=\underset{\tau -1}{\overset{t-1}{.Math.}}\underset{h-1}{\overset{K}{.Math.}}{a}_{t-\tau }\left(h\right)a_{\mathrm{hk}}N\left(\tau |{\mu }_k^D,{\sigma }_k^D\right)o_{\tau }^t$
allows to compute the sequence of marginally most probable states, while we are looking for the jointly most probable sequence of states given the last observation ξ.sub.T.sub.m. As a result, when using above formula there is no guarantee that the returned sequence s*.sub.T.sub.m will match both the spatio-temporal patterns of the demonstrations and the final observation. In terms of the example in FIG. 3, it may return the lower branch as the most likely sequence (i.e., to grasp the object from the side), even if the desired final configuration is that the end effector 22 is on the top of object.

(31) To overcome this issue, in step S3 a modification of the Viterbi algorithm is used. Whereas the classical Viterbi algorithm has been extensively used to find the most likely sequence of states (also called the Viterbi path) in classical HMMs that result in a given sequence of observed events, the modified implementation differs in that: (a) it works on HSMM instead of HMM; and that (b) most observations except the first and the last ones are missing.

(32) Specifically, in the absence of observations the Viterbi algorithm becomes

(33) ${\delta }_t\left(j\right)=\underset{d\in D}{\max }\underset{i\ne j}{\max }{v}_{t-d}\left(i\right)a_{\mathrm{ij}}{p}_j\left(d\right)\underset{t^{\prime }=t-d+1}{\overset{t}{.Math.}}{\tilde{b}}_j\left({\xi }_{t^{\prime }}\right){\delta }_1\left(j\right)=b_j\left(o_1\right){\pi }_j{p}_j\left(1\right)\mathrm{where}{\tilde{b}}_j\left({\xi }_{t^{\prime }}\right)=\{\begin{array}{cc}N\left({\xi }_{t^{\prime }}|{\hat{\mu }}_j,\underset{j}{\widehat{.Math.}}\right),& t=1\vee t=T\\ 1,& 1<t<T\end{array}$

(34) The Viterbi algorithm is modified to include missing observations, which is basically what is described for variable ‘b’. Moreover, the inclusion of duration probabilities p.sub.j(d) in the computation of variable ‘d_t(j)’ makes it work for HSMM.

(35) At each time t and for each state j, the two arguments that maximize equation δ.sub.t(j) are recorded, and a simple backtracking procedure can then be used to find the most probable state sequence s*.sub.T.sub.m.

(36) The above modified Viterbi algorithm provides the most likely state sequence for a single TP-HSMM model that produces the final observation δ.sub.T. As multiple skills are used, these models need to be sequenced and δ.sub.t(j) has to be computed for each individual model M.sub.a.sub.h. However, only ξ.sub.1 and ξ.sub.T can be observed, thus some models will not produce observations. An additional challenge emerges when sequencing HSMMs: as the state transition probabilities between subsequent HSMM states are unknown, the Viterbi algorithm cannot be applied directly to find the optimal state sequence.

(37) As a next step, symbolic abstractions of the demonstrated skills allow the robot to understand the meaning of each skill on a symbolic level, instead of the data level of HSMM. This may generalize a demonstrated and learned skill. Hence, the high-level reasoning of the herein described PDDL planner, needs to understand how a skill can be incorporated into an action sequence in order to achieve a desired goal specification starting from an initial state. A PDDL model contains the problem instance P.sub.p and the domain P.sub.D.

(38) While the problem P.sub.p can be easily specified given the objects O, the initial state and the goal specification G, the key ingredient for symbolic abstraction is to construct the actions description in the domain P.sub.D for each demonstrated skill, wherein P.sub.D should be invariant to different task parameters.

(39) a.sub.h∈A is a symbolic representation for one demonstrated skill in PDDL form. The learned TP-HSMM M.sub.a.sub.h contains a task-parametrized model
{π.sub.k,{μ.sub.k.sup.(p),Σ.sub.k.sup.(p)}.sub.p=1.sup.P}.sub.k=1.sup.K.

(40) For each model, it is then possible to identify two sets, each of which containing initial and final states and denoted by ,.Math.{1, . . . , K}, respectively.

(41) To construct the preconditions of a skill, the segments of demonstrations that belong to any of the initial state are to be identified, and to further derive the low-variance geometric relations which can be mapped into the set of predicates B. For each initial state i∈, its corresponding component in frame p is given by (μ.sub.i.sup.p,Σ.sub.i.sup.(p)) for p=1, . . . , P. These frames correspond to objects {o.sub.1, . . . , o.sub.P}, i.e., skill a.sub.h interacts with these objects. For each demonstration {ξ.sub.t.sup.(p)}.sub.t=1.sup.T.sup.m∈D.sub.a.sub.h, the most-likely sequence s*.sub.T.sub.m=s*.sub.1s*.sub.2 . . . s*.sub.T.sub.m can be computed as described above. If the first T.sub.i time steps correspond to state i, namely, s*.sub.1=s*.sub.2= . . . =s*.sub.T.sub.i=i, one instance of the predicate b.sub.l∈B can be evaluated at state i along this segment:
b.sub.l(o.sub.1, . . . ,o.sub.P)=T, if

(42) $\frac{\underset{t=1}{\overset{T_i}{.Math.}}{b_{\ell }}_{}\left(o_1^t,.Math.,o_P^t\right)=T}{T_i}>\eta$
where 0<η<1 is a design parameter (probability threshold). o.sub.1.sup.t, . . . , o.sub.P.sup.t are object states computed based on the recorded frame coordinates {b.sub.t.sup.(P),A.sub.t.sup.(p)}.sub.p=1.sup.P and object geometric dimensions. Denote by B.sub.i the set of instantiated predicates that are True within state i,∀i∈. As a result, the overall precondition of skill a.sub.h is given by the disjunction of the conjunction of these predicates for each initial state, i.e.,

(43) $\mathrm{PreCond}\left(a_h\right)=\underset{i\in 𝒥}{\vee }\underset{b\in B_i}{\wedge }b$
where ∇ and Λ are the disjunction and conjunction operations. Similarly, to construct the effect of a skill, the procedure described above can be applied to the set of final states . In particular, for each final state f∈, the set of instantiated predicates that are True within f is denoted by B.sub.f. However, in contrast to the precondition, the effect cannot contain a disjunction of predicates. Consequently, the effect of skill a.sub.h is given by

(44) $\mathrm{Effect}\left(a_h\right)=\underset{f\in ℱ}{\wedge }\underset{b\in B_i}{\wedge }b$
as the invariant predicates common for all of the final states. Based on the above elements, the PDDL model P can be generated in an automated way. More importantly, the domain P.sub.D can be constructed incrementally whenever a new skill is demonstrated and its descriptions are abstracted as above. On the other hand, the problem instance P.sub.p needs to be re-constructed whenever a new initial state or goal specification is given.

(45) Following, it is referred to the planning and sequencing of trained and abstracted skills. The PDDL definition P has been constructed, which can be directly fed into any compatible PDDL planner. Different optimization techniques can be enforced during the planning, e.g., minimizing the total length of the plan or total cost. Denote by a*.sub.D=a*.sub.1=a*.sub.2 . . . a*.sub.D the generated optimal sequence of skills, where a*.sub.d∈A holds for each skill. Moreover, denote by M.sub.a*.sub.d the learned TP-HSMM associated with a*.sub.d.

(46) Given this sequence a*.sub.D, each skill within a*.sub.D, is reproduced as the end-effector trajectory level, so to maximize the probability of satisfying the given goal G.

(47) The learned TP-HSMM encapsulates a general skill that might have several plausible paths and the choice relies heavily on the desired initial and final configurations. To avoid incompatible transitions from one skill to the next, a compatibility measure shall be embedded while concatenating the skills within a*.sub.D. Particularly, the proposed solution contains three main steps: a) Cascade the TP-HSMMs of each skill within a*.sub.D into one complete model {circumflex over (M)}.sub.a*.sub.D, by creating transition probabilities according to the divergence between the final states/components of the previous manipulation skill and the initial states of the current manipulation skill.

(48) Since the transition from one skill to another is never demonstrated, such transition probabilities are computed from the divergence of emission probabilities between the sets of final and starting states. Particularly, consider two consecutive skills a.sub.d* and a.sub.d+1* in α.sub.D* . The transition probability from one final state f of M.sub.a.sub.d* to one starting state i of M.sub.a.sub.d+1* is given by

(49) $a_{\mathrm{fd}}\propto \exp (-\alpha *\underset{p\in P_c}{\overset{}{.Math.}}\mathrm{KL}(N(\left({\mu }_f^{\left(p\right)},\underset{f}{\overset{\left(p\right)}{.Math.}}\right).Math.N\left({\mu }_i^{\left(p\right)},\underset{i}{\overset{\left(p\right)}{.Math.}}\right)))$
where KL(⋅∥⋅) is a KL-divergence (Kullback-Leibler-Divergence; see also Kullback, S.; Leibler, R. A., “On Information and Sufficiency.” Ann. Math. Statist. 22 (1951), no. 1, 79-86. doi:10.1214/aoms/1177729694, P.sub.c is the set of common frames between these two skills, and a≥0 is a design parameter. The outgoing probability of any final state should be normalized. This process is repeated for all pairs of starting and final states between consecutive skills in a.sub.D* . In this way, one complete model {circumflex over (M)}.sub.a.sub.D.sub.* has been created for the desired sequence of skills. b) Find the most-likely complete state sequence ŝ*.sub.T.sub.D within {circumflex over (M)}*.sub.a.sub.D given the initial and goal configurations. Given the derived complete TP-HSMM {circumflex over (M)}*.sub.T.sub.D, the observed initial state and the goal specification, the above modified Viterbi algorithm can be applied to find the most-likely state sequence ŝ*.sub.T.sub.D. c) Generate the robot end-effector trajectory that optimally tracks ŝ*.sub.T.sub.D, namely, to reproduce all skills in a*.sub.D. Given the states sequence ŝ*.sub.T.sub.D, a linear quadratic regulator (LQR) can be applied to generate the necessary control commands to reproduce the optimal state sequence ŝ*.sub.T.sub.D. A step-wise reference is obtained from {circumflex over (μ)}.sub.s.sub.t,∀s.sub.t∈ŝ*.sub.T.sub.D, which is tracked by the LQR.sup.1 using associated tracking precision matrices {circumflex over (Σ)}.sub.s.sub.t.sup.−1. As the robot state x lies in the Riemannian manifold M.sub.R=.sub.3×S.sup.3×.sup.1, the emission probabilities are therefore Riemannian Gaussian, and thus the step-wise reference is also given in M.sub.R. This implies that the required linear system dynamics for the end-effector 22 cannot be defined on M.sub.R, since it is not a vector space. However, the linear tangent spaces can be exploited to achieve a similar result. Specifically, the state error between the desired reference {circumflex over (μ)}.sub.s.sub.t and current robot state x.sub.t can be computed using the logarithmic map

(50) ${\log }_{{\hat{\mu }}_{s_t}}\left(x_t\right)$
that projects the minimum length path between {circumflex over (μ)}.sub.s.sub.t and x.sub.t into the Euclidean space

(51) 0${{\hat{\mu }}_{s_t}}_{}{M}_R.$
The covariance matrices {circumflex over (Σ)}.sub.s.sub.t describe the variance and correlation of the robot state variables in a tangent space

(52) ${{\hat{\mu }}_{s_t}}_{}{M}_R.$
Under the above assumptions, the control objective in the tangent space can be formulated as

(53) $c\left(u\right)=\int \left({{\log }_{{\hat{\mu }}_{s_t}}\left(x_t\right)}^T{\widehat{.Math.}}_{s_t}^{-1}{\log }_{{\hat{\mu }}_{s_t}}\left(x_t\right)+u_t^T{\mathrm{Ru}}_t\right)\mathrm{dt}$  where u is the control input and R regulates the control effort. To retrieve only the reference trajectory, it can be assumed double integrator dynamics, which provides smooth transitions between consecutive {circumflex over (μ)}.sub.s.sub.t.

(54) Finally, in step S4 the concatenated sequence of manipulation skills is executed.