Incremental building of a polyhedral feature model for ... - CiteSeerX

1

Incremental building of a polyhedral feature model for programming by human demonstration of force controlled tasks Peter Slaets, Tine Lefebvre, Herman Bruyninckx, Joris De Schutter Department of Mechanical Engineering, Katholieke Universiteit Leuven, Celestijnenlaan 300B, B3001 Leuven(Heverlee), Belgium. Email: [email protected]

Abstract— This paper describes the construction of a local polyhedral 3D feature model derived from pose and wrench sensor measurements collected during a force-controlled execution of a sequence of polyhedral contact formations. The procedure consists of a two steps: (i) the sensor measurements are filtered resulting in a (non-minimal) representation of a 3D feature model; (ii) identification of a reduced set of geometric parameters (vertices and faces) of the rigid polyhedral objects in the environment is performed. Following improvements with respect to the state of the art are made: (i) creation of a polyhedral 3D feature model of a previously unknown polyhedral environment, (ii) estimation of a force decomposition useful for feedback in a force controller or for monitoring the contact forces, (iii) reduction of the number of model parameters, leading to a computational reduction, a better precision of the geometric parameters and a higher level description of the contact topology. Index Terms— contact modeling, polyhedral 3D feature model, geometric modeling, compliant motion, estimation, Kalman Filter, programming by demonstration, model reduction, integrated likelihood, Bayes factor

I. I NTRODUCTION This paper describes the construction of a local geometric polyhedral 3D feature model using a two step procedure based on sensor measurements collected during an action executed by a human or a robot. This action is performed in an unknown environment while various sensors collect information about the action and environment. First the information is processed in a statistical manner leading to the gradual building of a task model which includes an explicit geometric 3D model of the surrounding objects. This geometric model is local, because it extends only as far as the measurement data allow it. This geometric model may be described by a non-minimal parameter representation. In a next step the number of parameters of this geometric model can be reduced by selecting more specific models in a Bayesian statistical manner. This paper focuses on compliant motion tasks in which a robot moves a manipulated object while keeping contact with the object(s) in its environment using force and position sensing and control. However, the approach is also useful for other applications, involving various types of sensors, e.g.: • world modeling: the creation of a virtual 3D environmental feature model for simulations.

• •

on-line adaptive control: the estimation of geometric contact parameters for adaptive force-controlled robots. programming by demonstration (PbD): the generation of a sensor-based robot program based on a human task demonstration.

The focus of this paper is PbD. The idea behind PbD is to reduce the effort spent in implementing complex computer code needed to program a sensor-based robot task. This reduction is achieved by transforming the sensor measurements of a demonstrated task into more abstract programming primitives. The subsequent autonomous re-execution of the task by the robot in a slightly modified environment requires robustness with respect to geometric variations. This robustness can be achieved by creating a geometric 3D model of the surrounding objects both during the human demonstration and during the re-execution by the robot. Fig. 1 shows the setup for an autonomous compliant Fig. 1.

A cube-in-corner assembly

motion task that the human demonstrates via a joystick. This setup is used to illustrate the effectiveness of our geometric modeling method; The robot holds the manipulated object (cube)and assembles it with an environmental object (corner) by executing a sequence of Contact Formations (CFs) [1]. A general contact state between two objects can be characterized topologically by the set of Principle Contacts (PCs); this set is called a contact formation (CF). PCs, i.e. vertex-face (vf ) contact, edge-face (e-f ) contact, face-face contact, etc., are used to describe the contact primitives between two surface elements of two polyhedral objects in contact, where a surface element can be a face, an edge or a vertex. Every PC can be decomposed into one or more Elementary Contacts (ECs), providing a lower level description of the contact formation as shown in fig. 2. An EC is a point contact consisting of a contact point and a contact normal. Three types of EC exist: face-vertex (f -v), vertex-face (v-f ) and edge-edge (e-e). This paper uses only the vertex-face EC. The manipulated object as well as the environmental object are assumed to be polyhedral but their actual geometry is unknown before the human task demonstration.

2

1a

2a

3a

1b

2b

3b

4a

5a

6a

4b

5b

6b

Fig. 2. A general contact formation (CF) can be decomposed into one or more elementary contacts (ECs), which are associated with a contact point and a contact normal. The dotted arrows indicate the edge-edge ECs, and the solid arrows indicate the vertex-face or face-vertex ECs.

A. Previous work on modeling of contact formations and estimation of geometric parameters During or after the task demonstration, an estimator (e.g., a Kalman filter [2]–[4]) estimates the unknown geometric model parameters by processing force and position measurements collected during the compliant motion task. To this aim the estimator needs predefined analytical measurement equations describing different contact models. Linking the measurements to the geometric model parameters of the CF models enables simultaneous recognition of CFs and estimation of their corresponding geometric parameters. Similar work was done by comparing maximum likelihood estimates of systems for which the configuration space is known [5] or by starting a set of deterministic estimators for each possible CF and use the results as measurements in a HMM [6]. Other techniques are based on learned contact models that make use of Hidden Markov Models (HMM) [7], fuzzy or neural network structures [8] or qualitative measurements [9] to define a model for the contact formations (CFs). The task parameters (not only geometric parameters) are determined in a second step, after a classification between the different models has been made. Another group of techniques make use of analytical contact models that do not depend on geometric parameters [10]. Some of them are based on qualitative template matching of each measurement component [11], [12], others use polyhedral cones to indicate the ranges of possible wrenches and twists measured during a CF [13]. Non contact optical surface modelling techniques (camera, scanner, ...) [14] focus on constructing a global geometric 3D model from a grid of measured points. Optical modelling techniques are complementary to the approach presented in this paper because they focus on constructing a global geometric 3D model of complex surfaces whereas an accurate local geometric modelling of polyhedral surfaces is achieved here.

To cope with large uncertainties of the geometric parameters, a non-minimal state Kalman filter [15] has been designed. This filter allows us to work with non-linear measurement equations combined with large uncertainties on the initial state estimates. This is apposed to other types of Kalman filtering (e.g. Iterated Extended Kalman Filter (IEKF) [16]) which highly depend on the starting values and the nonlinearities in the measurement model. An alternative to Kalman filters are particle filters that extend the statistical analysis to non-Gaussian distributed probabilities capable of processing all non-linear measurement equations [17]. B. Previous work by the authors In previous work, the authors presented a general approach to estimate first order geometrical parameters during a force controlled compliant motion [18]. These first order geometrical parameters include location of contact points and tangential directions of contacting surfaces for arbitrary non-polyhedral contact formations. This approach is based on a linearized recursive Kalman filtering algorithm leading to a stochastic estimation of the geometrical parameters. In addition, transitions between contact formations can be monitored using stochastic consistency tests derived from the Kalman filter. In a next step the modelling effort was significantly reduced, for the case of polyhedral contacts, by decomposing each confact formation into a set of elementary contact formations [19]. In addition the propagation of geometrical parameters estimates between contact formations was made straightforward for the case of contact between objects with a known geometry. Accurate parameter estimation however requires persistent excitation. This problem was investigated in [20] which uses an active sensing approach to generate a task plan that is optimized off-line to minimize the covariance of the geometric parameter estimates [21]. Both the sequence of contact formations and the compliant motion in each confact formation are optimized in this respect. For dealing with larger initial uncertainty on the geometrical parameters estimates a new type of Bayesian filter (NonMinimal State Kalman Filter) has been introduced [22]. This new filter has also been applied to other problems, such as curvature estimation, and hypothesis testing [23]. This hypothesis testing uses likelihood ratios and is applied to low dimensional problems. A more powerful hybrid discrete/continuous approach based on a particle filter has been developed for simultaneous recognition of contact formations (discrete part) and estimation of geometrical paramters (continuous part) [24]. In [25] this hybrid approach is scaled to a much larger number of contact formations, linked to a contact state graph generated by a compliant motion planning system [26] , and used for the segmentation in a human demonstration experiment. In particular, the contact formations modelled in the contact state graph are recognized and the corresponding geometrical parameters are estimated. C. Our contributions Whereas our previous work focuses on the recognition and estimation of contacts between known objects, this paper

3

applies some of these techniques to construct a local 3D polyhedral feature model of unknown objects in contact. This problem is relevant in the context of programming by human demonstration. We use the elementary contact models and the Non-Minimal State Kalman Filter of [22] as well as the hypothesis testing of [23]. This paper contains following contributions : 1) Previous work [19] focused on the estimation of the position and orientation of the environmental and manipulated objects when these are polyhedral and their geometry is known. Our approach is able to create a 3D feature model of all the contacting features of the objects when, additionally, the geometry is unknown a priori (but limited to polyhedral objects) . 2) This paper uses a consistency-based force measurement equation resulting in a force decomposition by the estimator. This decomposition can be used in PbD as force-setpoints that characterize the desired contact forces in each individual EC of the CF. Another application is to use the decomposition as feedback to a force controller which independently controls the contact forces and moments in each of the individual contacts during a task execution. 3) The estimates of the Kalman filter contain all relevant geometric information pertaining to the demonstrated task. However, the information is often represented by a nonminimal number of parameters. A method to reduce the number of parameters is developed. This leads to a computational reduction in the Kalman filter. Other advantages are a better precision of the estimates and a higher level description of the contact topology (Section IV). 4) An extra constraint that takes into account the limitations imposed by unilateral contacts has been added to the modeling process . 5) A Bayesian method [27], for high dimensional model selection is proposed. This method uses no approximations and is able to incorporate prior information into an incremental model-building process. II. M ODELING In order to estimate geometric parameters using wrench and pose measurements, the Kalman filter needs measurement equations that link these measurements to the geometric parameters. Section II-A describes the geometric parameters (xg ) that represent the contacting objects. This paper uses only v-f ECs where the vertex is attached to the manipulated object and the face is attached to the environment; models for the other elementary contact formations can be applied easily within this paper’s framework. Sections II-B and II-C explain the construction of the pose and wrench measurement equations. The wrench measurement equation introduces a force decomposition resulting in extra parameters (wrench coordinate vector φ). In the case of unilateral contacts additional constraints are imposed on the wrench coordinates (Section II-D). Section II-E presents a systematic procedure to combine the different measurement equations into a single model. Finally,

Section II-F explains a reduction of the non-minimal number of parameters present in the model by using geometrical constraints. A. Uncertainty parameters and measurements For a v-f EC the unknown geometric parameters are the position of the contacting vertex (p = [px , py , pz ]T ) and four parameters ([a, b, c, d]) describing the face using a nonminimal parametrisation of the Cartesian equation of a plane apx + bpy + cpz − d = 0.

(1)

This non-minimal parametrisation avoids singularities inherent to a minimal parametrisation e.g a0 = a/c, b0 = b/c, c0 = 1 and d0 = d/c when c = 0. The geometric parameters are collected in the state vector xg . The following six-vectors are measured and collected in a measurement vector z: 1. contact wrenches w = [Fx Fy Fz Mx My Mz ]T (forces F and moments M ), measured by a wrist force sensor; 2. poses td of the manipulated object (the positions and orientations) derived from the robot joint positions by the forward robot kinematics. B. Pose measurement The pose measurement equation relates the manipulated object pose measurement td to the geometric parameters xg : h(xg , td ) = 0.

(2)

This equation is constructed for every EC of the CF. The equations express that a contact is really established; loss of contact introduces errors in the equations. E.g., for a vf contact the pose measurement equation expresses that the vertex lies in the face by using equation (1). All parameters are expressed in frames where they are static e.g. for a v-f contact the vertex is attached to a moving gripper frame {g} and the face to a static world frame {w}. However in equation (1) the parameters need to be expressed with respect to the same frame. This is achieved using transformation matrices, which are nonlinear functions of the parameters, resulting in a nonlinear pose measurement equation (2). C. The wrench measurement equation The wrench space is the vector space of all possible contact wrenches given an ideal, i.e. rigid and frictionless contact [18]. It is represented by a base (G) and corresponding coordinate vector φ: w − Gφ = rm .

(3)

This residue measurement equation expresses the consistency of the measured wrench w with respect to the contact model, described by the base G [28], [29]. The residual vector rm represents that part of the measured wrench that is not explained by the first order kinematics of an ideal frictionless contact. Therefore in an ideal, rigid and frictionless contact situation the residual vector is equal to zero. Equation (3) introduces extra state variables φ. E.g. for a face-face CF and

4

with the wrench base expressed in a frame with its Z-axis perpendicular to the contacting faces:   0 0 0  0 0 0      1 0 0  φ1   w− (4)  0 1 0  φ2 = 0.   φ3  0 0 1  0 0 0 The first column of G represents the contact force (Fz ) perpendicular to the face while the last two columns represent the contact moments (Mx , My ) about the axes parallel to the face. The wrench space G corresponding to a CF is the union of the wrench spaces of all ECs in the CF. Every individual wrench base can be expressed in terms of the geometric parameters of both contacting objects. E.g. the wrench expressed in a contact frame with its origin in the contact point {c} of a v-f CF equals a pure force normal to the estimated plane (n = [a b c]T ) i.e. : Gc = [a b c 0 0 0]T . Hence in this case φ consists of only one coordinate. Because the wrench measurements are expressed in a gripper frame {g} and the wrench base is expressed in a contact frame {c}, a screw transformation cg S [30] is needed to transform the contact wrench base from the contact frame {c} to the gripper frame {g}. The multiplication with a screw transformation matrix introduces a non-linearity in the wrench measurement equation (3). D. Extra constraint for unilateral contacts Equation (3) does not take into account the limitations imposed by unilateral contacts. In each unilateral v-f contact i the wrench coordinate φi is either positive or negative, corresponding to the direction of the contact normal. The sign of each φi is derived from the velocity of every estimated contact point prior to contact (vi ) and from the orientation of the corresponding estimated contact normal (ni ): sign(ni · vi ) < 0 =⇒ φi > 0 or abs(φi ) − φi = 0;

(5)

sign(ni · vi ) > 0 =⇒ φi < 0 or abs(φi ) + φi = 0.

(6)

Constraints (5) and (6) are combined into one additional measurement equation for all elementary unilateral v-f contacts: hφ (φ) =

k X

(sign(φi )φi + sign(ni · vi )φi ) = 0.

(7)

i=1

E. Systematic modeling of polyhedral CFs Every CF is described by s ECs (v-f , f −v, e−e) in parallel. This results in s scalar pose measurement equations (1) and a single wrench measurement equation (3) which corresponds to six scalar equations . All measurement equations are a function of the geometric parameters:    h1 (xg , td ) = 0; .. (8) .   hs (xg , td ) = 0,

hw (xg , td )

= w − G(xg , td )φ = 0.

(9)

In case of unilateral contacts, an extra constraint equation (7) has to be added. Using this procedure any polyhedral CF can be modelled. The geometric parameters of the s ECs collected in xg , and the wrench coordinate vector, φ, are combined into the state vector x = [xTg φT ]T . On the other hand the pose measurements, td and the wrench measurements, w, are combined into a measurements vector z = [tTd wT ]T . All measurement equations are then represented by h(x, z) = 0.

(10)

For example: the edge-face PC model consists of two v-f EC pose measurement equations (8) and six scalar wrench measurement equations (9) together with a unilateral constraint equation (7). This PC model uses fourteen parameters describing two vertices (6) and two faces (8). Transition to another CF corresponds either to a contact gain or a contact loss. In the case of a contact gain, a pose equation (1), with corresponding geometric parameters has to be added; in case of a contact loss, a pose equation, with corresponding geometric parameters, has to be removed. This systematic procedure is only useful if it is possible to represent every CF using a non-minimal number of ECs, without bothering about whether this results in a minimal state vector or not. Therefore, the estimator has to recognize the measurement equations as being non-minimal and has to ensure that the information is not processed more than once. Section III-A describes a filter that can deal with this type of measurement equations. F. Non-minimal vs. reduced CF representation The CF model consists of a non-minimal number of geometric parameters. A reduction of the number of geometric parameters is achieved by imposing geometric constraints that follow from assumptions or hypotheses about the geometry. We illustrate this with the edge-face PC of Section II-E which consists of two v-f ECs. The non-minimal model, which assumes two independent v-f contacts, contains 2 × 7 = 14 parameters. A first constraint is to assume that both faces coincide. This imposes the following constraints: a1 = a2 , b1 = b2 , c1 = c2 and d1 = d2 . In addition, since both vertices lie in the same face, the second vertex is completely defined by only two independent coordinates. Both contact points are derived from a vertex-face EC model and therefore they are called vertex contacts. Geometrically they actually represent a point that lies on an edge. Hence, a second constraint, pz1 = pz2 , follows from a proper choice of the reference frame for expressing the vertex coordinates. (e.g. a frame whose origin coincides with the gripper frame {g}, but with its Z-axis normal to the face). Application of both constraints reduces the number of parameters from fourteen to nine. This reduced model with only nine parameters is shown in figure 3. Furthermore, if knowledge about the orientation of the edge is available, one independent coordinate suffices to completely define the second vertex. Hence the number of

5

where

parameters further reduces to eight.

III. PARAMETER ESTIMATION A Kalman filter is used to estimate the state vector x. The system model consists of a process model used for the process update (III-A.1) and a measurement model used for the measurement update (III-A.2). In Kalman filtering both are assumed to be linear. Non-linear measurement models are approximated by a model that is linearized in the current state estimate or next state estimate, leading to the Extended Kalman filter and the Iterated Extended Kalman filter (IEKF) (III-A), respectively. The use of an IEKF or EKF is only justified when the estimates are close to the real values. More robustness with respect to this limitation is achieved by using, the non-minimal state Kalman filter (NMSKF) [31], Section III-B. The NMSKF defines a new state vector (x0 = f (x)) that result in ”linearized” measurement equations (h(x0 , z) = Hx0 + c) which can be solved using a linear Kalman filter without approximation errors. This filter is used to conduct the experiments presented in Section V. The detection of contact transitions between CFs is performed by a SNIS-test (III-C). The detection of transient behavior, that is present after a contact transition, is performed by a test explained in (III-D). A. Kalman filter algorithm The Kalman filter algorithm is easy to use, has low computational cost and low memory requirements. This estimator keeps track of the state estimate x and its covariance matrix P . It has been shown [32] that this type of filter is robust against non-minimal measurement equations. The Kalman filter algorithm consists of two update steps: 1) Process update: The process model updates the state estimates in-between measurements in order to follow the dynamics of the system. The part of the state corresponding to the geometric parameters xg is static (i.e. the real value of the parameters does not change over time). These parameters can however become ’too accurate’ (small covariances) after a number of measurement updates resulting in numerical instability and low adaptability of the estimates. A limitation on the magnitude of the covariance matrix of the state is implemented to prevent this. On the other hand the part of the state corresponding to the wrench coordinates φ can not be predicted due to the absence of a contact dynamics model. The process model for this part (φ) of the state makes the estimate uncertain again by imposing a large covariance Pφ,0|0 on the estimate at every process update (the next estimate only depends on the current measurement). This leads to the following equations: xk+1|k Pk+1|k

= Axk|k + µ; T

= APk|k A + Q;

(11) (12)

Fig. 3. A non-minimal representation of an edge-face CF composed by fourteen parameters: [a1 b1 c1 d1 px1 py2 pz1 a2 b2 c2 d2 px2 py2 pz2 ] (figure on the left) compared to a minimal representation composed by nine parameters: [a1 b1 c1 d1 px1 py1 pz1 px2 py2 ] (figure on the right).

[xT φT ]T ; µ = [0T φ0|0 ]T ; g    I 0 0 0 = ;Q = . 0 0 0 Pφ,0|0

x =

(13)

A

(14)

2) The measurement update: This step updates the state estimate based on a measurement z. The measurement equations of a v-f contact are nonlinear and implicit i.e. of the form h(x, z) = 0. Hence a linearisation around the next or current state estimate will be required, resulting respectively in an Iterated Extended Kalman filter and a Extended Kalman filter. The exact formulas can be found in [16] using equations (7), (8), (9). B. The cascaded Kalman filter A filter that can deal with any kind of nonlinear measurement equations has been proposed by [19] for static systems subjected to large Gaussian uncertainties on the initial states estimates. The cascaded filtering process consists of two steps: in a first step a Non-Minimal State Kalman filter (NMSKF) is used to determine a (not necessarily Gaussian) probability density function (pdf) of the state space (x); in a second step an IEKF is used to approximate this pdf by a normal distribution. 1) Non-Minimal State Kalman filter: In order to calculate the posterior pdf over x, we define a higher dimensional state1 x0 = g(x) for which the measurement equations are linear. This leads to the following process and measurement model: x0k+1|k 0 Pk+1|k 0 0

h (x , z)

= A0 x0k|k + µ0 ; = A =

0

0 Pk|k A0T

(15) 0

+Q;

0.

(16) (17)

Note that the measurements are still non-linear in z. This filtering process results in an estimate of the posterior pdf over x: p(x|zk ) ∼ N (x0 = g(x)|µ0k , Pk0 )

(18)

where N is a Gaussian pdf in x0 with mean µ0 and covariance P 0 . For example: a non-linear measurement equation h(u, v, w, z) = z1 uv+z2 vw+z3 wu = 0 is converted into a linear measurement equation h(u0 = uv, v 0 = vw, w0 = wu) = z1 u0 + z2 v 0 + z3 w0 = 0 that can easily be solved using an IEKF resulting in a Gaussian pdf in (u0 , v 0 , w0 ) but a non-Gaussian pdf in (u, v, w). 2) IEKF filter: In the second step an IEKF is used to transform the non-Gaussian pdf in x from the higher dimensional space x0 to the original lower dimensional space in x. An IEKF gives a good approximation of the expected value and covariance if the measurements fully observe the system, [32]. At every time step, an IEKF is constructed with 1 When dealing with a limited number of different measurement equations, we can always find a state transformation x0 = g(x) which linearizes them. E.g. for N different measurement equations hi (x, z) = 0 we can choose a transformation x0 = [g1 (x) g2 (x) ... gN (x)]T resulting in linearized measurement equations h(x0 , z) = H(z)x0 + c(z).

6

a mean (xk ) and a large covariance Pk . The measurements (z) are equal to the mean of the non-minimal state estimate (z = µ0k ). Hence the covariance on the measurements equals the covariance of the non-minimal state Pk0 . The corresponding process and measurement equations are: xk+1|k

= Axk|k + xk

with

T

Pk+1|k

= APk|k A + Q

h(x, z)

= g(x) − µ0k = 0.

A = 0;

(19)

with Q = Pk ;

(20) (21)

The prior pdf of the state x is important because this second filter is an IEKF that converges only when the estimates stay close to the real value. A good initial estimate is obtained by setting the minimal state (x) equal to the current estimate of the non-minimal filter (µ0 ). Due to the non-minimal representation of a plane, a singularity exists if the geometric parameters a, b, c and d are all equal to zero. The IEKF may converge to this solution, but it has no physical meaning. To prevent this, an extra measurement equation is introduced for each v-f contact : 1 − (a2 + b2 + c2 ) = 0. Combined with (21) this yields the new measurement equations:   g(x) − µ0k h(x, z) = = 0. (22) 1 − (a2 + b2 + c2 ) C. Detecting contact transitions The SNIS-test [3] provides a consistency check of the measurements with respect to the measurement equations in the Kalman filter. The SNIS is a sum over the NIS-values of the last m time steps. These NIS-values represent a weighted version of the squared innovations (η = h(xk , zk )). The innovations correspond to the evaluation of the measurement equations h(xk , zk ) = 0 given the current estimate (xk ) and the current measurement (zk ). This weighting is performed by a covariance matrix Sj representing the uncertainty expected on the innovations. SN IS =

j=k+1 X

ηjT Sj−1 ηj

(23)

j=k−m+2

The SNIS value is χ2 distributed with m × nh degrees of freedom, where nh is the number of statistically independent measurement equations in the current model and m is the number of NIS-values that are used in the SNIS formula. When a transition between CFs occurs, the SNIS value will reach a value that is above the chosen statistical boundary of the χ2 distribution. Hence the SNIS-test provides a criterion to detect transition between CFs. In order to prevent these measurements from disrupting the Kalman estimation the last m estimations are thrown away. D. Detecting transient behavior During the transient behavior between two CFs the measurements are useless for the Kalman filter. To cope with this problem the parameter estimations and their covariances

are kept constant. For Gaussian wrench uncertainties, transient behaviour is detected by monitoring the following number: k+1 X

(wi − w ¯i )Rc−1 (wi − w ¯i )

(24)

i=k−nc +2

This sum is χ2 distributed with zero mean and nc × 6 degrees of freedom, where 6 is the number of statistically independent normally distributed measurements (wi ). Rc is the value of the wrench measurement covariance matrix and nc is a chosen parameter that determines the speed of the detection criterion, i.e. a small value results in a fast detection of transient behavior whereas a large value results in a slow detection. The mean (w ¯i ) is calculated over the last nc wrench measurements. When a transient occurs, the monitored value will reach a value that is above the chosen statistical boundary of the χ2 distribution. IV. M ODEL REDUCTION The aim of model reduction is to model every nonelementary contact model using a reduced number of geometric parameters (Section II-F). In turn, this leads to a significant computational reduction together with a gain in accuracy. The selection of an appropriate contact model that uses as few parameters as possible is done using a hypothesis tester that is able to compare the probability of one model against others. The model hypotheses used during the cube in corner experiment are described in Table I. Numerous methods exist to compare different models. They can be divided into two major classes, the likelihood based approximations, and the likelihood ratio test statistics. Two types of information criteria are used for likelihood based approximations: the Kullback-Leibler (KL) information criterion [33] and a Bayesian-based information criterion [34] (Section IV-A). A. Bayes factor The Bayes factor information criterion uses the marginal likelihood of the measurements: Z +∞ p(z) = p(z|x)p(x)dx. (25) −∞

This Section uses an exact Bayesian method based on (25) that incorporates no approximations and is able to incorporate prior information into an incremental model-building process. Section IV-B describes the integration of this exact Bayesian method into the NMSKF. The Bayes factor (B) is the posterior to prior odds ratio, i.e.: p(M1 |z)/p(M2 |z) p(z|M1 ) B= = . (26) p(M1 )/p(M2 ) p(z|M2 ) The Bayes factor that compares one model against another model is the posterior odds for that model against the other, when neither model is favored a priori (p(M1 ) = p(M2 )). The posterior is equal to the marginal likelihood for each model. This yields the following advantages: (1) it provides a way of incorporating prior knowledge about

7

the statistical uncertainty of the estimate; (2) it is very useful for guiding an incremental model-building process; (3) it does not suffer from approximation errors. These advantages are discussed in detail below. (1) Since the Bayes factor is based on the marginal likelihood (25) it compares different models, without fixing to a particular choice of the parameter value, but rather it averages over all possible parameter values using the prior distribution. (2) Evolutionary model building is made easy because there is no limitation on the parameter space of the possible hypotheses that can be compared. This is a great advantage over nonBayesian techniques that have large difficulties comparing non-nested models [35], [36]. (3) A Kalman filter produces at every moment an estimate of the state parameters p(x|M, z1,..,k ), which can be used as prior distribution before a new measurement is processed. This joint probability density can be written as: p(x, z|M, z1,...,k ) = p(z|x, M )p(x|M, z1,..,k ),

(27)

where p(z|x, M ) = L(x) is called the likelihood of model M given the state x and p(x|z1,...,k , M ) is the prior knowledge concerning the states of model M . An integration of this joint probability density with respect to the state leads to an exact marginal likelihood (25). This integral is calculated by making a Laplace approximation of the logarithm of this joint density around the current state estimate: log(p(x, z|M, z1,..,k )) = log(p(ˆ x, z|M, z1,..k )) + . . . G(ˆ x, z)(x − x ˆ) − 0.5(x − x ˆ)T H(ˆ x)(x − x ˆ),

(28)

where G(ˆ x, z) equals the gradient and H(ˆ x) represents the negative Hessian of log(p(x, z|M, z1,...,k )) evaluated at x = x ˆ. This approximation is exact for a normally distributed posterior. In our case we have such a normally distributed posterior function because an IEKF is used to estimate this posterior. The current estimate of x ˆ is the maximum posterior probability estimate of the joint density due to the use of Kalman filter estimations. Therefore the estimate satisfies G(ˆ x, z) = 0. Hence, the marginal likelihood of formula (25) results in the integration of a scaled normal distribution with mean x ˆ and covariance matrix equal to the inverse of the Hessian H(ˆ x): = p(ˆ x, z|M, z1,..k ) × . . . Z ∞ T (2π)0.5d e−0.5(x−ˆx) H(ˆx)(x−ˆx) dx

p(z|M, z1,...,k )

(29)

−∞ −0.5

= p(ˆ x, z|M, z1,..k )(2π)0.5d det H(ˆ x) Here d represents the dimension of the state space. B. Implementation of the Bayes factor into the NMSKF

New measurement models corresponding to hypotheses can be inserted into the first or the second step of the filtering process (NMSKF or IEKF step). Insertion into the first step implies that every additional hypothesis requires the start of an additional NMSKF. On the other hand insertion into the second step implies that the estimated posterior (18) is re-used in every IEKF (one for each hypothesis) . This yields

a computational advantage that is our motivation for choosing this type of hypothesis testing.

C. Overview of the complete estimation procedure The complete estimation procedure described in Section III and IV is summarized below. This procedure consists of five steps. (1) Transient behaviour is monitored using the wrench measurement and equation (24). During a transient the measurements are not used for updating the state estimates, that is, we do not proceed to step 2. If there is no transient, go to step 2. (2) The NMSKF is started using equations (15-17) and pose and wrench measurements to update the state estimate. The measurement equation (17) corresponds to equations (7-9). Initially the measurement model represents an elementary v-f contact. (3) Contact transition is monitored using the SNIS value (23). If a transition is detected an extra EC is added to the polyhedral CF model (Section II-E) and the last m estimations are thrown away. Therefore we go back to the step 1 of m iterations before. Else we proceed to step 4. (4) If a primitive model has been selected in a previous iteration, go to step 5. Otherwise, proceed as follows. Different IEKF’s are started using equations (19-21). The measurements for the IEKF’s correspond to the NMSKF state estimate. The measurements (21) equations correspond to different primitive models. The most plausible reduced model is selected using the Bayes factor (Section IV-A). Go to step 1. (5) Different IEKF’s are started using equations (19-21). The measurements for the IEKF’s correspond to the NMSKF state estimate. The measurements equations (21) correspond to different reduced models. The most plausible reduced model is selected using the Bayes factor (Section IV-A). Go to step 1. V. E XPERIMENTS Experimental data is collected from an assembly of a cube with side l = 250 mm in a corner. The task is executed by a KUKA 361 industrial robot arm with a 6D JR3 force/torque wrist sensor (see figure 1). In the experiment this compliant motion is specified manually using the Task Frame Formalism [37] and is executed by a hybrid force/position controller. The experiment could equally well have been performed by a human controlling the force controlled robot with a joystick. The Task Frame Formalism requires an implicit model of the task geometry. The aim of the experiment is to reconstruct this model (and hence make it explicit) based on wrench and pose measurements collected during the experiments and using the estimation procedure outlined in Sections III and IV. Persistent data excitation is achieved by imposing trajectories that sequentially excite all the remaining degrees of freedom during contact (initially six degrees of freedom: three rotations and three translations). The geometric parameters of both the manipulated object (cube) and that of the environment object (corner) are supposed to be unknown. The initial position of

8

the cube is such that no e-e or e-v CF contact formations occur during the assembly process. The estimates of the geometric parameters are obtained by off-line processing the measurements of wrenches and poses collected at 10 Hz, see figures 7 and 8. The estimators are able to process the measurements on-line2 , however this has not yet been integrated in to our current control software. After detection of a new CF the set of pose and wrench measurement equations (8) and (9), as well as the extra constraint for unilateral contacts (7) are adapted. This detection is currently performed manually by measuring inconsistencies between the data and the measurement equations using a SNIS-test (III-C). However this detection will be automated in the near future.

estimations at the end of the filtering process.

•

The first 200 measurements are collected during a free space motion. These measurements are omitted from figure 5 and 6 because they contain no information about the geometric parameters.

•

The first contact is a v-f contact. It starts at measurement 0 and ends at measurement 746. Subplots E1 and E2 in figure 6 show the evolution of the estimated parameters of the face and the vertex respectively. The estimated vertex (v1 ) and face (f1 ) at the end of the CF (i.e. just before transition to the edge-face contact), are added to the 3D model (figure 5: CF1 ). The vertex is shown by its estimated position and its 2σ uncertainty ellipsoid.

•

The second CF starts at measurement 764 and ends at measurement 1830. It consists of two v-f ECs. After hypothesis testing using a Bayes factor (Section IV-A), an edge-face contact model becomes the most plausible. This allows a reduction of the number of parameters from fourteen (two times seven) to eight: seven parameters from the first contact (figure 6, E1 and E2 ) and one new parameter describing the inaccurately known position of the second contact (v2 ) along edge e1 . The NMSKF, based on two v-f EC models, imposes no limitations on the position of vertex v2 along the edge, leading to a large uncertainty on the estimate in this direction. This uncertainty results in an unstable estimate for this position in the IEKF. The edge (e1 ) containing v1 and v2 is added to the 3D model (figure 5, CF2 ).

•

The third CF, that starts at measurement 1837 and ends at measurement 2518, consists of three v-f ECs. After hypothesis testing a face-face contact becomes the most plausible CF. This results in a reduction of number of parameters from twenty-one (three times seven) to ten: eight from the edge-face contact and two new parameters that describe the inaccurately known position of the contact (v3 ) in the face of the manipulated object (figure 5, CF3 ).

•

The fourth CF, that starts at measurement 2535 and ends at measurement 3720, adds a new v-f to the previous face-face model. After hypothesis testing an additional v-f becomes the most plausible, it lies in a plane of the cube that goes through the edge e1 and perpendicular to the xy-plane of the reference frame. This results in a reduction of the number of parameters from twenty-eight to fifteen: ten parameters describing the face-face contact and five new parameters that describe a new vertical environmental face f2 (figure 5, CF4 , figure 6, E3 ) and the position of the fourth contact v4 (figure 5, CF4 , figure 6, E4 ). This position expressed in the reference {g} frame is very well determined in x and y directions (E4 = px4 and py4 = py1 ) and uncertain in the third direction (z).

A. Initial estimates Although the geometric parameters are initially unknown the estimator needs initial estimates and corresponding covariances. For the position of the vertices we take the origin of a gripper frame g (px = 0, py = 0, pz = 0)3 with covariances: σp2x = l2 , σp2y = l2 and σp2z = l2 . Furthermore an initial estimate of (a, b, c) is derived from the previous estimate of the NMSKF; d4 is set to zero. The corresponding covariances are chosen as σa2 = σb2 = σc2 = 22 , and σd2 = 5l2 . In order to cope with linearisation errors, inherent to every Kalman filter estimator, the experimentally determined pose and wrench measurement standard deviations are multiplied by 5. This factor was determined experimentally. In free space the noise on the force sensor measurements is approximately 0.1 N. The wrench measurements are very sensitive to friction effects, these effects result in a maximal friction force of approximately 5 Newton (N) for a contact force of 28 N. This unmodeled friction is taken into account by multiplying the standard deviation of the wrench measurements by a factor 50. B. Global results The experiment consists of the following sequence of CFs: v-f (CF1 ), e-f (CF2 ), f -f (CF3 ), e-f in combination with f -f (CF4 ), two f -f contact formations, three f -f contact formations (CF5 ) (see figure 1). The first five CFs are recognized and modeled as shown in figure 5 and 6. Figure 5 visualizes the estimated vertices (vi ) and faces (fi ), as obtained at the end of each CF. Figure 6 shows these estimates as a function of the processed measurements. Only the geometric parameters that are relevant to the geometric model building process are visualized (i.e. the parameter estimates that have a large uncertainty are omitted). Figure 6 also quantifies the numerical accuracy of the 2 Processing all measurements by a C++ program an a Pentium 4.2 GHz, 500 Mb RAM laptop goes faster than the measurement frequency (10Hz) if the number of elementary contact formations is smaller than four. 3 Units are mm 4 Unit is mm

9

•

The last CF, that starts at measurement 3735 and ends at measurement 4116, adds an extra v-f contact to the previous model. After hypothesis testing we conclude that the fifth vertex (v5 ) lies in the second face (f2 ) and in the previously described vertical face of the cube, therefore the position of the vertex in this face is very uncertain in the other two directions (figure 5, CF5 ).

C. Results of segmentation and hypothesis testing Segmentation is performed using the SNIS criterion explained in Section III-C. In this application the number of NIS-values is chosen S = 6 and the tolerated statistical boundary is 0.5%. The SNIS-values during the measurements of the five different CFs are shown in figure 4. During a CF all SNIS-values stay under the statistical confidence boundary, as visualized by a horizontal line, and peak above the boundary every time a new CF is detected. The transient behavior is detected using the consistency criterion explained in Section III-D. Here parameter nc of equation (24) is chosen nc = 6 and the statistical boundary is set at 60 %. Because there is no predefined sequence of CF, a transition can indicate the loss or the gain of a contact; even a false alarm (staying in the same CF) is possible. To make a selection between the different models we have to find the probability of each CF and compare them. This is done using the hypothesis tester of Section IV-A, whose results are shown in figure 9. The four subplots at the top show the results of hypothesis testing after the occurrence of a transient T Ri (i = 1, . . . , 4). After each transient T Ri the parameter vector is augmented with seven parameters, representing a new v-f contact. Subsequently we test if the estimates of these new parameters represent a new v-f contact (hypothesis 1) or converge to an already existing v-f contact (hypothesis Hj , j = 2, .., i + 1). Table I lists these hypotheses for every transient T Ri . The four subplots at the bottom of figure 9 show the results of hypothesis testing within a contact formation CFi (i = 1, . . . , 5). Each time several hypotheses are formulated in order to find a reduced model. Again table I lists these hypotheses for every CFi . For example, we explain the different steps (Section IVC) to obtain the reduced model after the detection of transient T R2 (step 1) at measurement 1837. Before the transient the model consists of an e-f contact. Three primitive models, corresponding to hypotheses H1 , H2 and H3 become plausible. Only the first one states that a new contact v3 has been added. The primitive model that becomes considerably more plausible is selected (figure 9, subfigure T R2 ), i.e. H1 . Hence this primitive model is selected (step 2). Next three hypotheses corresponding to reduced models are formulated and compared in figure 9, subfigure F2 . The first hypothesis (H1 ) imposes two constraints: v3 lies in f1 of the corner and v3 in the ground face of the cube. The second hypothesis only imposes the first constraint (H2 ) while the third one imposes no constraints (H3 ). Finally the most plausible hypothesis (H1 ), which best represents the contact model, is selected.

Fig. 4. SNIS confidence test. The confidence boundary is chosen at 0.5% and the first 200 measurements describing the motion in free space (SNIS=0) are omitted.

D. The wrench decomposition Equation (9) can be used to validate the obtained geometric model. At each time step k, wrench measurements wk can be compared to a model based estimation: west,k = G(xg,k , td,k )φk ,

(30)

where td,k represents the pose measurement at time step k, xg,k represents the estimates of the geometric parameters and φk represents the estimates of the wrench coordinates at time step k. Figure (7) shows the results. The minor difference between wk and west,k illustrates the validity of the constructed model and points out the feasibility to use the estimates of the wrench coordinates in a wrench feedback loop. VI. D ISCUSSION The following restrictions and assumptions have been made in our approach: (1) Assumptions are made regarding the geometry of the contacting objects (polyhedral) and the possible contact situations (vertex-face). An extension to other principal contact situations (face-vertex and edge-edge) is straightforward, whereas an extension to non-polyhedral CFs results in more complex models. This is subject of ongoing research. (2) Persistent excitation is needed to obtain accurate parameter estimates. In the experiment, persistent data excitation is achieved by executing trajectories that sequentially excite all the remaining degrees of freedom in any contact formation. This excitation scheme might be difficult to execute in the context of PbD (i.e. not intuitive for a human demonstrator). However by providing online visual feedback of the estimated uncertainty of the geometric parameters, the human demonstrator can be made aware of the persistent excitation problem, and can be given hints about new excitation trajectories. This online visual feedback is subject of on-going research. This type of data excitation does not imply a prior knowledge about the trajectory but an additional task during execution. During a human demonstration persistent data excitation could be monitored using a online visual feedback of the estimated uncertainty of the geometric parameters to the demonstrator. (3) The selection of plausible primitive and reduced models is performed manually. However, automation can be achieved by analyzing the covariance matrix of the estimates and the SNIStest and linking this to a database of possible CF transitions and reduced models. (4) The estimation procedure is robust with respect to a change parameter settings Rc and P0|0 only if the measurements fully observe the system. Sufficient data excitation during the demonstration results in a convergence of the state estimate i.e. the measurement fully observe the system. The convergence of the state estimates depends on the initial uncertainties on the measurements (Rc ) and state (P0|0 ). That is high initial

10

parameter a1 b1 c1 d1 px1 py1 pz1 a2 b2 c2 d2 px2

real value 0 0 1 680 [mm] 125 [mm] 125 [mm] 315 [mm] -1 0 0 -1000 [mm] -125 [mm]

estimated values 0.0024 0.0040 1 684.2 [mm] 130.136 [mm] 126.4425 [mm] 317.9342 [mm] -0.9991 0.0372 0.0394 -1041.2 [mm] -123.1753 [mm]

2x standard deviation 0.0017 0.00008 0.00006 6.5211 [mm] 1.3551 [mm] 1.1649 [mm] 6.3659 [mm] 0.00007 0.0035 0.0224 19.0577 [mm] 1.2260 [mm]

Fig. 5. Visualization of the constructed geometric 3D model (vertices, planes) together with the unknown reference cube and of the final numerical results. CF1 : a v-f contact formation with vertex (v1 ) and face (f1 ). CF2 : an e-f contact formation with an extra vertex (v2 ) on the edge (e1 ). CF3 : a f -f contact formation with an extra vertex (v3 ) leading to an estimated face of the manipulated object (f10 ). CF4 : a f -f and v-f contact formation with an extra vertex (v4 ) leading the estimation of a new face of the environment (f2 ). CF5 : a f -f contact formation combined with two v-f contact formations (v4 , v5 ) in the same face f2 . Bottom right: estimates of the geometric parameters and their corresponding accuracy (2x standard deviation) are compared with the real values. The vertices (v1 , v2 , v3 , v4 , v5 ) are expressed in mm and in a gripper frame {g}, the faces (f1 ,f2 ) are expressed in a world frame {w} shown in figure CF1 . Different viewpoints together with an automatic scaling have been chosen for every figure in order to obtain the best visualization of the different features. Fig. 6. State estimations (solid lines) and twice their standard deviation (dashed lines) and their real values (dashed-dotted lines). E1 : the parameters of the ground plane of the corner (f1 , with d1 [mm] ), E2 [mm] the coordinates of the vertex v1 , E3 the parameters of a vertical plane (f2 , with d2 [mm]) of the corner, E4 [mm] the x coordinate of the fourth v-f contact Fig. 7.

Measured wrench w (solid line) and model based reconstruction of the wrench west (dashed lines).

uncertainties require more data excitation than lower initial uncertainties. (5) During the example no contact loss has occurred, and therefore these hypotheses have been omitted. VII. C ONCLUSION In this paper a local geometric feature model of the task environment is constructed using information that is present in force and pose measurements collected during human demonstration of a force-controlled compliant motion. The conducted experiment shows that the geometric parameters as well as the contact configuration transitions of moderately complex contact situations can be estimated (semi)autonomously. The presented approach is generic, based on Bayesian theory, and capable of working with large initial estimation errors. In addition, it can perform automatic model reduction, i.e., it can detect whether a set of elementary v-f contact formations belong together to form a “higher-level” contact formation, such as e-f or f-f. Our algorithms are not limited to the particular task we have demonstrated in the experiment, except for the fact that a “model library” of only polyhedral contact formations is used. . Our main short term research objective is to integrate this estimation process into a real-time controlled robot system in order to perform monitoring of a compliant motion without the need for a model of the environment. In addition application to PbD leads to a robustness with respect to environmental changes. This robustness is achieved by the systematic construction of a 3D feature model from the measurements

collected during the demonstration. This feature model can be used as prior information during the autonomous execution leading to improved robustness with respect to changes in the environment. Next we will focus on the automatic generation of all plausible hypotheses by taking into account all the collected knowledge about the environment. This requires the update of an explicit data structure that represents the geometric model of the environment and the manipulated object. A further step is to extend the contact models to non-polyhedral CFs. ACKNOWLEDGMENT All authors gratefully acknowledge the financial support by K.U.Leuven’s Concerted Research Action GOA/05/10. R EFERENCES [1] Jing Xiao, “Automatic determination of topological contacts in the presence of sensing uncertainty,” In Proceedings of the 1993 IEEE International Conference on Robotics and Automation [38], pp. 65–70. [2] R. E. Kalman, “A new approach to linear filtering and prediction problems,” Transactions of the ASME, Journal of Basic Engineering, vol. 82, pp. 34–45, 1960. [3] Y. Bar-Shalom and X. Li, Estimation and Tracking, Principles, Techniques, and Software, Artech House, 1993. [4] Arthur (Ed.) Gelb, Optimal Estimation, MIT Press, Cambridge, MA, 3rd edition, 1978. [5] Brian Eberman, “A model-based approach to Cartesian manipulation contact sensing,” The International Journal of Robotics Research, vol. 16, no. 4, pp. 508–528, 1997. [6] T. Debus, P. Dupont, and R. Howe, “Contact State Estimation using Multiple Model Estimation and Hidden Markov Models,” in Proceedings of the International Symposium on Experimental Robotics, Sant’Angelo d’Ischia, Italy, 2002, in press.

11

Fig. 8.

The pose [Px Py Pz θz θy θz ] measurements collected during the cube in corner experiment at 10 Hz.

figure T R1 F1

T R2

F2

T R3

F3

T R4

F4

Contact formation CF2 CF2 CF2 CF2 CF2 CF2 CF3 CF3 CF3 CF3 CF3 CF3 CF4 CF4 CF4 CF4 CF4 CF4 CF4 CF5 CF5 CF5 CF5 CF5 CF5 CF5 CF5

hypothesis H1 H2 H1 H2 H3 H4 H1 H2 H3 H1 H2 H3 H1 H2 H3 H4 H1 H2 H3 H1 H2 H3 H4 H5 H1 H2 H4

description new vertex contact v2 v2 = v1 v2 lies on the edge e1 of the cube and the face f1 of the corner v2 lies in the ground plane of the cube and face f1 of the corner v2 lies in face f1 of the corner no hypothesis new vertex contact v3 v3 = v2 v3 = v1 v3 lies in ground face of the cube and f1 of the corner vertex lies in face f1 of the corner no extra hypothesis new vertex contact v4 v4 = v3 v4 = v2 v4 = v1 v4 lies in XZ oriented plane of cube (in {g} frame) through edge e1 v4 lies on an edge vertical through v2 vertex lies in YZ oriented plane (in {g} frame) of the cube through v2 new vertex contact v5 v5 = v4 v5 = v3 v5 = v2 v5 = v1 v5 lies a XZ oriented plane of cube (in {g} frame) and in f2 plane v5 lies in f2 plane no extra hypothesis

TABLE I TABLE I: DESCRIPTION OF THE HYPOTHESES TESTED IN FIGURE 9. T HE INTEGRATED LIKELIHOODS OF THE DIFFERENT HYPOTHESIS TESTS (H1 , .., H4 ) AFTER A TRANSITION

(T R1 , .., T R4 ) OR DURING A KNOWN CONTACT FORMATION (F1 , .., F4 ) ARE VISUALIZED IN FIGURE 9.

Fig. 9. This figure gives an overview of the conducted Bayes tests: the left figures compare the different contact formations that are possible after a transition has been detected (T R1 , ..., T R4 ), the right figures compare different reduced models during an established contact formation (F1 , ..., F4 ). The hypothesis with the largest log(probability) is the most plausible (in this case always H1). The different hypotheses are explained in table I.

[7] B. Hannaford and P. Lee, “Hidden Markov Model analysis of force/torque information in telemanipulation,” The International Journal of Robotics Research, vol. 10, no. 5, pp. 528–539, 1991. [8] M. Skubic and R. Volz, “Identifying single-ended contact formations from force sensor patterns,” IEEE Transactions on Robotics and Automation, vol. 16, no. 5, pp. 597–603, October 2000. [9] Qi Wang, J. De Schutter, Wim Witvrouw, and Sean Graves, “Derivation of compliant motion programs based on human demonstration,” in Proceedings of the 1996 IEEE International Conference on Robotics and Automation, Minneapolis, MN, 1996, pp. 2616–2621. [10] H Asada, “Representation and learning of nonlinear compliance using neural nets,” IEEE Transactions on Robotics and Automation, vol. 9, no. 6, pp. 863–867, Dec 1993. [11] B. J. McCarragher, “Force sensing from human demonstration using a hybrid dynamical model and qualitative reasoning,” in Proceedings of the 1994 IEEE International Conference on Robotics and Automation, San Diego, CA, 1994, pp. 557–563. [12] Pierre E. Dupont, Timothy M. Schulteis, P. Millman, and Robert D. Howe, “Automatic identification of environment haptic properties,” Presence: Teleoperators and Virtual Environments, vol. 8, no. 4, pp. 392–409, 1999. [13] S. Hirai and H. Asada, “Kinematics and statics of manipulation using

[14] [15]

[16] [17]

[18]

[19]

the theory of polyhedral convex cones,” The International Journal of Robotics Research, vol. 12, no. 5, pp. 434–447, 1993. F. Blais, “Review of 20 years of range sensor development,” Journal of Electronic Imaging, vol. 13, no. 1, pp. 231–240, 2004. Tine Lefebvre, Klaas Gadeyne, Herman Bruyninckx, and Joris De Schutter, “Exact Bayesian inference for a class of nonlinear systems with application to robotic assembly,” in Bayesian Statistics 7, J. M. Bernardo, M. J. Bayarri, J. O. Berger, D. Heckerman A. P. Dawid, A. F. M. Smith, and M. West, Eds., pp. 587–596. 2003. Hisashi Tanizaki, Nonlinear Filters. Estimation and Applications, Springer-Verlag, 1996. A. Doucet and V. B. Tadic, “Parameter Estimation in General StateSpace Models using Particle Methods,” Annals of the Institute of Statistical Mathematics, vol. 55, no. 2, pp. 409–422, 2003. J. De Schutter, H. Bruyninckx, Stefan Dutr´e, Jan De Geeter, Jayantha Katupitiya, Sabine Demey, and T. Lefebvre, “Estimating first order geometric parameters and monitoring contact transitions during force controlled compliant motion,” The International Journal of Robotics Research, vol. 18, no. 12, pp. 1161–1184, 1999. T. Lefebvre, H. Bruyninckx, and Joris De Schutter, “Polyhedral contact formation modeling and identification for autonomous compliant motion,” IEEE Transactions on Robotics and Automation, vol. 19, no.

12

1, pp. 26–41, 2003. [20] Tine Lefebvre, Herman Bruyninckx, and Joris De Schutter, “Task planning with active sensing for autonomous compliant motion,” The International Journal of Robotics Research, vol. 24, no. 1, pp. 61–82, 2005. [21] Jan De Geeter, J. De Schutter, Hendrik Van Brussel, and Marc Decr´eton, “A smoothly constrained Kalman filter,” IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 19, no. 10, pp. 1171–1177, 1997. [22] Tine Lefebvre, Herman Bruyninckx, and Joris De Schutter, “Polyhedral contact formation identification for autonomous compliant motion: Exact nonlinear Bayesian filtering,” IEEE Transactions on Robotics and Automation, vol. 21, no. 1, pp. 124–129, 2005. [23] Tine Lefebvre, Herman Bruyninckx, and Joris De Schutter, “Online statistical model recognition and state estimation for autonomous compliant motion,” IEEE Transactions on Systems, Man, and Cybernetics: Part C, vol. 35, no. 1, pp. 16–29, 2005. [24] Klaas Gadeyne, Tine Lefebvre, and Herman Bruyninckx, “Bayesian hybrid model-state estimation applied to simultaneous contact formation recognition and geometrical parameter estimation,” The International Journal of Robotics Research, vol. 24, no. 8, pp. 615–630, 2005. [25] Wim Meeussen, Johan Rutgeerts, Klaas Gadeyne, Herman Bruyninckx, and Joris De Schutter, “Contact state segmentation using particle filters for programming by human demonstration in compliant motion tasks,” Tech. Rep., K.U.Leuven, Leuven, Belgium, 2006. [26] Wim Meeussen, Joris De Schutter, Herman Bruyninckx, Jing Xiao, and Ernesto Staffetti, “Integration of planning and execution in force controlled compliant motion,” in Proceedings of the 2005 IEEE/RSJ International Conference on Intelligent Robots and Systems, Edmonton, Canada, 2005, pp. 2550–2555. [27] Harold Jeffreys, Theory of Probability, Clarendon Press, 1939, 2nd edition, 1948; 3rd edition, 1961. Reprinted by Oxford University Press, 1998. [28] H. Bruyninckx, J. De Schutter, and S. Dutr´e, “The “reciprocity” and “consistency” based approaches to uncertainty identification for compliant motions,” In Proceedings of the 1993 IEEE International Conference on Robotics and Automation [38], pp. 349–354. [29] M. S. Ohwovoriole and B. Roth, “An extension of screw theory,” Transactions of the ASME, Journal of Mechanical Design, vol. 103, no. 4, pp. 725–735, 1981. [30] J. A. Parkin, “Co-ordinate transformations of screws with applications to screw systems and finite twists,” Mechanism and Machine Theory, vol. 25, no. 6, pp. 689–699, 1990. [31] T. Lefebvre, H. Bruyninckx, and Joris De Schutter, “Exact nonlinear Bayesian parameter estimation for autonomous compliant motion,” Advanced Robotics, vol. 18, no. 8, pp. 787–800, 2004. [32] Tine Lefebvre, Herman Bruyninckx, and Joris De Schutter, “Kalman Filters for nonlinear systems: a comparison of performance,” International Journal of Control, vol. 77, no. 7, pp. 639–653, 2004. [33] Solomon Kullback and R. A. Leibler, “On information and sufficiency,” The Annals of Mathematical Statistics, vol. 22, pp. 79–86, 1951. [34] G. Schwarz, “Estimating the dimension of a model,” Annals of Statistics, vol. 6, pp. 461–464, 1978. [35] D.R. Cox, “Tests of separate families of hypotheses.,” in Proceedings of the Fourth Berkeley Symposium on Mathematical Statistic and Probability. 1961, pp. 105–123, University of California Press. [36] D.R. Cox, “Further results on tests of separate families of hypotheses.,” Journal of the Royal Statistical Society, vol. B, no. 24, pp. 406–424, 1962. [37] J. De Schutter and H. Van Brussel, “Compliant robot motion I. A formalism for specifying compliant motion tasks,” The International Journal of Robotics Research, vol. 7, no. 4, pp. 3–17, 1988. [38] Proceedings of the 1993 IEEE International Conference on Robotics and Automation, Atlanta, GA, 1993.