A Method for Constraint Inference Using Pose and Wrench Measurements

Many physical tasks such as pulling out a drawer or wiping a table can be modeled with geometric constraints. These geometric constraints are characterized by restrictions on kinematic trajectories and reaction wrenches (forces and moments) of objects under the influence of the constraint. This paper presents a method to infer geometric constraints involving unmodeled objects in human demonstrations using both kinematic and wrench measurements. Our approach takes a recording of a human demonstration and determines what constraints are present, when they occur, and their parameters (e.g. positions). By using both kinematic and wrench information, our methods are able to reliably identify a variety of constraint types, even if the constraints only exist for short durations within the demonstration. We present a systematic approach to fitting arbitrary scleronomic constraint models to kinematic and wrench measurements. Reaction forces are estimated from measurements by removing friction. Position, orientation, force, and moment error metrics are developed to provide systematic comparison between constraint models. By conducting a user study, we show that our methods can reliably identify constraints in realistic situations and confirm the value of including forces and moments in the model regression and selection process.

A Method for Constraint Inference Using Pose and Wrench Measurements Guru Subramani, Michael Hagenow, Michael Gleicher, Member, IEEE and Michael Zinn, Member, IEEE Abstract-Many physical tasks such as pulling out a drawer or wiping a table can be modeled with geometric constraints. These geometric constraints are characterized by restrictions on kinematic trajectories and reaction wrenches (forces and moments) of objects under the influence of the constraint. This paper presents a method to infer geometric constraints involving unmodeled objects in human demonstrations using both kinematic and wrench measurements. Our approach takes a recording of a human demonstration and determines what constraints are present, when they occur, and their parameters (e.g. positions). By using both kinematic and wrench information, our methods are able to reliably identify a variety of constraint types, even if the constraints only exist for short durations within the demonstration. We present a systematic approach to fitting arbitrary scleronomic constraint models to kinematic and wrench measurements. Reaction forces are estimated from measurements by removing friction. Position, orientation, force, and moment error metrics are developed to provide systematic comparison between constraint models. By conducting a user study, we show that our methods can reliably identify constraints in realistic situations and confirm the value of including forces and moments in the model regression and selection process.
Index Terms-Geometric constraints, kinematics, teaching by demonstration

I. INTRODUCTION
T ASKS performed by robots frequently include constraints. For example, a manipulator is restricted to linear motion when pulling out a drawer. Representing physical interactions as constraints provides information about the permissible motion of objects in the environment and the permissible directions that forces and moments may be applied to them. This knowledge provides a way to inform the robot to restrict its movements to allowable configurations so it does not violate this closed kinematic chain (e.g., to plan motion consistent with the constraint [1]- [3]) and to apply safe levels of force against the constraint to achieve the appropriate taskspecific interaction (e.g., through hybrid control [4]).
Information about the constraints in the task is valuable to program a robot, but current approaches to determine constraint parameters are not straightforward. For example, commercial robotic systems provide hybrid control abilities, however the constraints and their geometry must be determined manually. This makes such control difficult to use, limiting the ability of such systems to perform tasks involving constraints. One solution is to infer constraints from a human demonstration, which eliminates the need for explicit programming [5], [6]. This paper presents an approach to  Demonstration of an espresso making task using instrumented tongs consisting of sliding an espresso cup, pulling out a drawer, and actuating an espresso lever. Center: Inferred constraints in task. Our approach infers the samples associated with each constraint (segmentation), the type of constraint (model selection) and model parameters of each constraint (model regression) in the task.
infer constraints in human demonstrations of recorded force, moment, position, and orientation measurements.
The requirements of a constraint inference system may vary dramatically depending on application. In manufacturing environments, the tool and environment geometry may be known beforehand, but other applications may require interacting with unknown objects. If a system is to perform inference using a natural demonstration (e.g the espresso making task in Fig. 1), it may also need to handle demonstrations involving multiple constraint interactions, short constraint interactions, and other physical interactions such as picking up and placing objects.
Past work, summarized below, has shown that geometric constraint models can be fit from a demonstration containing sufficient data, however limited attention has been given to the ability to recognize constraints in a short demonstration. For example, the method described in [7] effectively learns arXiv:2010.15916v1 [cs.RO] 29 Oct 2020 a constraint model and develops a policy to act in the nullspace of a constraint, which allows mapping of motion across constraints. To accomplish this, the constraint model is trained using 20 seconds of recorded demonstration data. Particularly when a task is performed in a natural way, there is no guarantee a model can be fit with the available duration of demonstration data. Certain applications might require a constraint inference method that minimizes the data required for fitting.
Representations of constraints can be general (i.e., a broad model that fits many types of constrained motion) [1], [8] or can be derived from geometric primitives [9]. A general model offers flexibility to learn a wider range of constraints without specification of additional models, but is difficult to interpret. On the other hand, primitive models are more easily understood by human compatriots. For example, a rotational motion can be interpreted by an operator, whereas a general model or combination of low-level restrictions on rotational or translational motion may have little semantic meaning. Great effort [10]- [13] has been devoted to constraint fitting with known object geometries (e.g., polyhedral, cylindrical, and spherical tools) by combining kinematic and wrench relationships to fit the constraints of the interaction, including simultaneous active constraint interactions. Similarly, our method applies wrench-based fitting to our previous methods [14], [15] using nonlinear regression of constraint parameters, which allow for parameterization even when the interacting objects are not known.
The contribution of this paper is in presenting and assessing a constraint inference approach that uses both kinematic and wrench information to infer geometric constraints in a human demonstration, even if the objects are unknown. Our approach can take a recording of a demonstration and determine what constraints are present, when they occur, and their parameters. It can reliably identify and localize a variety of constraint types, even when the constrained interactions are short. We build upon earlier work [14], [15] that uses only kinematics to fit constraint models involving unknown objects and uses forces and moment information for validation. More specifically, the new method extends our previous work by providing a systematic review of the value of wrench information throughout the constraint inference process, extending to new constraint models, and providing a more thorough and formalized explanation of the constraint inference approach. We provide new technical method details which are validated through new, extensive experimental results.
In our new method, we incorporate reaction forces and moments in an optimization objective to fit constraint models. We describe an approach to resolve reaction wrenches from wrench measurements which also contain constraint specific friction wrenches. Our method requires minimal assumptions about the friction model and does not require any specific friction parameters. We define a general set of position, force, orientation, and moment error metrics that are used to compare fit models with measured data. The product of these contributions is a constraint inference approach that explicitly recognizes the physical interaction that occurs when interacting with a constraint, where motions and wrenches are related in a manner dictated by the physics of the constrained interaction. Our approach takes advantage of both wrench and kinematic measurements which enables improved performance over our previous methods that use only kinematics to fit constrained interactions with unknown objects. We assess our approach with a human subjects study with 9 participants that confirms both the approach's performance as well as the value of including wrench information directly into the nonlinear fitting of constraints, even if there are unknown objects. II. CONSTRAINT INFERENCE OVERVIEW This paper provides a framework to infer geometric constraints in human demonstrations. Our approach is to first partition the human demonstration into constrained motion and unconstrained motion, fit all candidate constraint models to each constrained motion segment, and finally select the appropriate constraint models from the fit models. We organize the constraint inference as three sequential steps: 1) Demonstration segmentation: Applications may require determining when constraints occur in a demonstration, especially when demonstrations represent a larger task. We discuss methods to segment the constrained motion from demonstrations in Section VI (after model regression and model selection to cover the more important contributions first). Our approach takes advantage of the reaction forces and moments that exist during constraint interaction. 2) Model regression: In Section IV, we show how nonlinear least squares regression can be used to fit geometric constraint models to kinematic and wrench measurements by leveraging the principle of virtual work. Our method derives reaction forces and moments from mea-sured wrenches by considering contributing dynamics from friction. This is described in Section III-C. We show that by simultaneously fitting with kinematic and wrench data, we are able to fit certain constraints using significantly less data compared to fitting with kinematic data (see Fig. 2). 3) Model selection: Once all models are fit to the constrained segments, each model fit must be evaluated and the appropriate model must be selected. The regression objectives are inadequate to perform this comparison because they evaluate to different units and scales. To address this problem, kinematic and wrench error metrics are derived for the general scleronomic constraint model to provide a consistent approach to evaluate models. This is described in Section V. III. MATHEMATICAL MODELING OF CONSTRAINTS Our constraint inference approach requires defined mathematical representations of the constraints. We use models based on human interpretable geometric primitives to represent constrained motion such as the planar constraint (e.g. a flat object wiping against a table), point-on-plane constraint (e.g. the motion of a pen on paper), and axial rotation constraint (e.g. the motion of a hinged door). These constraints are represented as mathematical equations that the tool or object must satisfy when under the influence of the constraint. The constraint models and relationship between the reaction wrenches (reaction forces and moments) imparted on constrained objects is described below.
When a robot end effector or other object moves under the influence of a constraint, the degrees of freedom of its rigid body motion are restricted to a subset in SE(3). These restrictions may be represented as mathematical relationships between defined geometry on the body (e.g., a point on the body) and defined global geometry (e.g., a plane in the environment). For example, consider a pen moving against paper. The motion of the pen tip is constrained to the planar surface of the paper. In this example, the rigid body is the pen, the location of the pen tip is the defined geometry on the rigid body, and the planar surface represents the global geometry. Geometric constraint models consist of equations that define the possible configurations of the rigid body.
Geometric constraints impose restrictions on the configurations on the rigid body. Applying forces and moments on a constrained rigid body creates reaction forces and moments between the rigid body and the constraint. The principle of virtual work expresses that reaction forces and moments from the constraint do not produce work. This property can be used to determine the permissible reaction forces and reaction moments of the constraint. It is also necessary to consider additional forces that may act on the rigid body such as friction, inertia, and gravity. These concepts are developed below.

A. Geometric constraint models
In our formulation, we consider scleronomic constraints, which are geometric constraint models that only impose restrictions on position and orientation (i.e. generalized coordinates of the rigid body) and do not vary with time [16]. In this paper we consider the eight constraint models shown in Fig. 3. These constraints were chosen as examples of commonly occurring geometric primitives. Our approach can be extended to other models satisfying the scleronomic constraint formulation. The constraint models considered here include: • Point-on-line constraint: The point-on-line constraint is equivalent to drawing a line against a straight edge. A point on the rigid body is constrained to a line in space. • Planar constraint: The planar constraint is exemplified by an eraser moving against a whiteboard. The rigid body can only rotate about a vector perpendicular to the plane, and all points within the rigid body translate parallel to this plane. • Prismatic constraint: The prismatic constraint represents translational motion in one direction. It is similar to pulling out a drawer. All points on the rigid body translate identically. • Axial rotation constraint The axial rotation constraint is similar to a door knob or a hinged door. All points on the rigid body rotate about an axis and translations are not permitted. This constraint is also referred to as a revolute joint. • Relaxed constraints: The planar, axial, and prismatic constraints impose restrictions on the orientation of the rigid body. In certain cases, the demonstration tool may not rigidly fix the orientation of the manipulated rigid body. For example, when grabbing an object with a parallel gripper, there is a slip direction where the rigid body may rotate. The relaxed versions of these constraints remove restrictions on the orientation of the constrained object in order to model this type of behavior.
The detailed constraint mathematical models are developed in Appendix A. However, to provide context we present an extended description of the point-on-plane constraint development in the following section.

1) Extended Description for Point-on-plane Constraint
Consider a 6-degree of freedom rigid body of negligible inertial properties located in space through translational coordinates r ∈ R 3 and orientation coordinates represented by a unit quaternion q ∈ Spin(3). The measurement reference frame is the fixed frame on this rigid body. The rigid body is constrained by k constraint equations Φ : SE(3) ⇒ R k : The scleronomic constraint model represented by equation (1) may be instantiated with a specific constraint model (e.g. with a point-on-plane constraint) with associated model parameters α α α which configure the constraint (e.g. the position and orientation of the plane). Thus, Φ becomes Φ(r, q, α α α).
The point-on-plane constraint describes a point on the rigid body constrained to a plane in the environment such as a pencil tip (point) moving across paper (plane). An illustrated representation of this constraint is shown in Fig. 4.
A plane may be specified by applying a general displacement (i.e. translation and rotation) transformation of the x-y plane which involves:  1) translating the x-y coordinate plane along the z-axis 2) rotating the translated plane about the origin.
We represent the rotation transformation using two exponential coordinates w = [w x , w y , 0] ∈ R 2 corresponding to ew ∈ SO(3), the exponential map, which is equivalent to a rotation matrix with an axis of rotation in the x-y plane. Rodrigues' rotation formula [17] is used to compute ew.w is the skew symmetric matrix similar to equation (62) in Appendix B. The third term of w is zero because rotations about the z -axis (perpendicular to the plane) do not alter the plane's geometry. The translation is represented by d ∈ R. The normal vector on this plane is represented by ew[0 0 1] T and the shifted origin of the x-y plane is represented by A point P on the plane satisfies the following equation: which specifies that the dot product between a vector within the plane and the plane normal is zero. Consider a point s * ∈ R 3 (in the global reference frame) defined as a point fixed to the rigid body and constrained to the plane. s is a vector directed from the origin of the measurement reference frame to the point s* (vector defined in the global reference frame). The local reference frame counterpart of s is − s (whose values do not change with the motion of the measurement frame): where A(q) is the orthogonal rotation matrix equivalent to the rotation represented by q.
The constraint is satisfied when s * is constrained to the plane, i.e. equation Φ : SE(3) ⇒ R 1 is: The parameters of this constraint are α α α = (s, d, w x , w y ). The derivation of the mathematical models of the other constraints shown in Fig. 3 is given in Appendix A.

B. Reaction forces and moments
In addition to the constraint models, our method requires that we develop a relationship between reaction wrenches and the constraint models so that all measured data can be used in the constraint fitting process. The formulation used is standard in multi-body dynamics literature [18] and is provided here as a review.
We determine permissible reaction forces and moments using virtual displacements and the principle of virtual work.
The first partial derivatives of constraint equations (1) are given by: where Φ r and Φ q are the partial derivatives of (1) and (δ r, δ q) are the virtual displacements. Equation (5) may also be written using virtual rotation variable δ π π π [18]. The virtual rotation variable is related to the angular velocity of a rigid body. This relationship is similar to how the virtual displacement δ p is related to linear velocity.
δ Φ = Φ r δ r + Φ π π π δ π π π = 0 (6) Equation (6) is similar to the Pfaffian constraint form used in the robotics literature [19]. The permissible reaction forces F r ∈ R 3 and reaction moments N r ∈ R 3 that the constraint applies to the constrained body must satisfy the virtual work equation [20] because constraint reaction forces and moments do not produce work. The principle of virtual work requires: Combining equations (6) and (7) using the Lagrange multiplier theorem [18]: where λ λ λ ∈ R k are the Lagrange multipliers for the k constraint equations. Equations (8) and (9) provide a relationship between the constraint equations Φ and the permissible reaction forces and moments. Φ π π π δ π π π is computed from Φ q δ q. This is provided in Appendix B. C. Resolving reaction forces and moments from wrench measurements Equations (8) and (9) relate the reaction forces and moments with the constraint equations described by equation (1). One challenge is that reaction forces and moments are not measured directly. Interactions with the physical environment involve additional forces such as friction, inertia, gravity, forces from object compliance, and forces from task-specific impedances. To fit the constraint models, we need to estimate the reaction wrenches from measurement wrenches containing these additional effects.
To do so, we assume quasi-static motion and choose to model only friction in our analysis of constrained motion. In cases where demonstrations are highly dynamic, such as involving objects with large inertia and acceleration, this approach may not be directly applicable and such dynamic effects must be modeled and incorporated appropriately.
Quasi-static motion against constraints generates frictional forces due to the sliding contact between surfaces. Forces attributed to friction oppose the motion of the constrained object. For example, when writing on paper (point-on-plane constraint), friction force opposes the motion of the pen tip. Friction forces may manifest themselves differently for each constraint and there is no general way to describe them.
However, friction forces and moments can be modeled for each constraint with certain assumptions. Our basic intuition is to remove friction in the direction of motion of objects in contact. We derive a relationship between the reaction forces and moments with the measured forces and moments as functions of the parameters of the constraint: where the measured quantities are forces (F), moments (N), linear velocity (v), and angular velocity (ω ω ω) and the parameters of the constraint are α α α. The specific formulation of equations (10) and (11) for each constraint model is provided in Appendix A. The resolving of reaction forces and moments for the point-on-plane constraint is provided here as an example.

1) Resolving reaction forces and moments for the point-onplane constraint
The goal is to resolve the reaction wrenches from the measured wrenches (i.e. determine expressions for F r (r, q, v, ω ω ω, F, N, α α α) and N r (r, q, v, ω ω ω, F, N, α α α)). We assume that inertia of the constrained object is negligible and the only wrenches acting on the object are friction and reaction wrenches.
The force and moment balance equation for the rigid body at the measurement frame are as follows: where F is the measured force, F µ is the friction force, F r is the reaction force, N is the measured moment, N µ is the friction moment, and N r is the reaction moment acting at the measurement frame. The measured force and moments are expressed in the global frame (i.e., rotated from the original force torque sensor measurements). For the point on plane constraint seen in Fig. 4, point s * on the rigid body is in contact with the plane. s * moves with velocity v s . The friction force acts at point s * on the rigid body and is directed along v s ( v s is a unit vector in this direction). Any force in the direction of v s is assumed to be friction. F µ is the friction force located at point s * directed along v s : where v s is given by: and s is the vector from the measurement frame to point s * in the global frame.
We assume that at s * , no moment is transferred (including friction moment) between the rigid body and the plane. The moment induced by the friction forces at s * is given by: Combining equations (12) through (16) results in expressions for the reaction forces and moments as a function of measured values: With equations to resolve reaction forces and moments, Equations 8 and 9 can be evaluated using measured data. Similar mathematical relationships between measured forces and moments and reaction forces and moments for the other constraint models are given in Appendix A.
IV. LEAST SQUARES REGRESSION OF CONSTRAINT

MODELS
Using the constraint and reaction wrench equations developed in the previous sections, the constraint model parameters can be fit to the measured data. The generalized constraint equations given in (1) are used to formulate a kinematic error objective while equations (8) and (9) are used to formulate a wrench error objective. The two error objectives are cooperative and are included in a summed objective to fit recorded demonstration data to a constraint model. Let the parameters of the constraint be α α α. and let the i th sample of data be represented with a tuple consists of measured poses, wrenches, and velocities estimated from the kinematic measurements. The error objectives are defined across the N samples of data p := {p 1 , ..., p N }. The kinematic and wrench error objectives are described below.

A. Kinematic error objective
The kinematic error objective is developed to incorporate kinematic measurements during fitting and is given by: where is the L(2) norm and w := {w 1 , ..., w N } are the weights on each sample. Sample weights allow increasing the influence of certain samples over others. Some samples included in the regression may not be part of the constraint or may be caused by measurement errors and should have less influence when fitting. For the kinematic error objective, only (r, q) is required from tuple p.

B. Wrench error objective
A wrench error objective is developed to incorporate wrench measurements during fitting. Equations (8) and (9) provide a relationship between the constraint equations and the applied forces and moments in a demonstration. To apply equations (8) and (9) for regression, the Lagrange multipliers must be determined jointly with the parameters of the constraint. This may be formulated as a weighted least squares objective: (8) and (9) are linear in λ λ λ and may be solved using linear least squares regression. The least squares error of Aλ λ λ = b is denoted as LSQ(A, b) where A and b are given by: where the reaction forces and moments, F r and N r , are evaluated from measured forces and moments using equations (10) and (11). Equation (20) may be reformulated as: The reaction forces and reaction moments are jointly estimated by including their expressions in the optimization objective.

C. Maximum likelihood estimate
Using the kinematic and wrench error objective functions we can determine the most likely model parameters α α α given model Φ and demonstrator data p. The maximum likelihood estimate of model parameters α α α based on observations p iŝ α α α MLE and can be estimated as: Since the two objectives W error and K error are physically related through virtual work, they behave cooperatively. Experimental evaluation indicates that weighting the two objectives has little effect on the overall fit. Equation (24) can be used to estimate the constraint parameter values. D. Robust regression Segmentation error and non-gaussian noise from sensor measurements may cause the nonlinear least squares optimization in equation (24) to fit poorly. All of the samples may not be part of the constraint. To address these issues, we use iterative reweighted least squares (IRLS) [21] to estimate the sample weights associated with the constraint jointly with the model parameters which helps to reject outliers.
The approach initializes equal weight values to each sample. It performs an optimization using a standard non-linear optimization algorithm (in this work we use BFGS [22]) using these weights. It calculates the errors associated with each sample and down weights samples that have large error. A re-weighting step is used to normalize the weights using a regularization function to prevent aggressive down-weighting of samples so that all samples continue to have an influence on the objective. Using these new weighted values, the algorithm performs another iteration of optimization.
The process of finding the most likely value of α α α is given in Algorithm 1. An error convergence exit criteria is not necessarily applicable since all models are fit to each constraint segment. In this work, we choose a basic exit criteria of maximum number of iterations N. A more advanced criteria could be considered in future implementations.
To illustrate the benefit of the iterative reweighted least squares we consider the example of moving a chess piece across a chessboard (shown in Fig. 2). In this case, we intentionally segment the demonstration incorrectly so a portion of the demonstration is in free space. When fitting the model without iterative reweighted least squares (IRLS) regression, the fit plane is offset due to outliers caused by incorrect segmentation. After 100 iterations of IRLS the fit converges to the Algorithm 1 Iterative reweighted least squares regression 1: procedure ROBUSTREGRESSION(X, w, N) 2: initialize(w) set all weights to 1 3: for j = 1 ... N do number of iterations 4: w← w / sum (w) Normalization 5: α α α ← FitFunction(X,w) BFGS minimization 6: E← ErrorFunction(X,α α α,w) vector of fit errors 7: return α α α true constraint model (see Fig. 5). With IRLS, we can estimate the constraint parameters from imperfect measurement data.  V. GEOMETRIC CONSTRAINT MODEL SELECTION Once the parameters have been fit for a set of candidate models, it is necessary to identify the model most consistent with the measured data. The difficulty of the model selection problem is a function of the number of candidate models and the uniqueness of these models.
To simplify the model comparison process, we create a single selection metric that can be used across models. In our formulation, the optimization fit error is not appropriate as the models consist of regressor functions that are not guaranteed to have consistent units. To allow for comparison, kinematic and wrench error metrics are defined in terms of measured quantities (m, rad, N, Nm). These metrics are compared to predefined thresholds and are combined into a single error metric by counting measurements that exceed these thresholds. We refer to this process as error tallying.

A. Error metrics
Position and force information allow for the construction of separate kinematic and wrench error metrics. An error metric is defined as a measure of distance between a measurement (e.g.r, q, F, N) and the constraint model.

1) Kinematic error metrics
Given that a constraint can enforce both a position and an orientation, there is a need for two kinematic metrics. A position distance error metric, D r (Φ(r * , q * ), r, q), and an orientation distance error metric, D q (Φ(r * , q * ), r, q), are defined for a pose measurement (r, q) ∈ SE(3) where (r * , q * ) is coincident with the constraint (satisfies Φ). Evaluation of (r * , q * ) consists of finding the point on the constraint that is closest in overall distance to the measured point. Determining these values is posed as a least squares regression where (r * , q * ) are solved simultaneously: where W is a large weight (1000) on the constraint equations to ensure that r * and q * are fixed to the constraint. ∠ is the rotation angle of a quaternion, represents quaternion multiplication, and con j represents the conjugate of a quaternion. ∠ con j(q) q * computes the angle between q and q* (bounded to 180 degrees). While the position and orientation have different units and bounds, we did not add any scaling or normalization factors as for many of the constraints, the position and orientation optimize independent aspects of the constraint.
The kinematic error metrics, D r and D q can subsequently be computed as: 2) Wrench error metrics Measured forces and moments provide metrics that quantify inconsistency with the constraint model, Φ. By computing the Lagrange multipliers individually for each measurement (see Section IV-B), a force and moment error can be written as the residual of the virtual work equations previously defined in equations (8) and (9). The values are normalized with respect to the measured forces and moments to remove a dependence on magnitude, resulting in the wrench error metrics, E f and E n , associated with the measured forces and measured moments respectively : 3) Combined error metric For model selection, we desire a single metric capable of disambiguation between candidate models. A weighted sum of the individual error metrics would contain inherent bias dependent on the range of motion and level of applied forces and moments. To circumvent this, we introduce a combined error metric (CE) which is a function of the number of samples of the constrained motion that exceed predefined error thresholds given as r SE , q SE , f SE , and n SE . The combined metric considers all measured variables and is normalized such that models can be compared directly. The error thresholds, which have units corresponding to the units of the corresponding error metric, are determined by evaluating the error metrics using the numerical values of the measurement error, which is a function of the experimental setup. The combined error metric (summed over the measured samples, n) is evaluated as: Section VII-C describes how the thresholds are set for each of the error metrics based on an estimate of the experimental measurement accuracy. Our combined error metric provides one method that allows combination of the different error metrics, but does require thresholds based on an estimate of system error. Future work could include a systematic comparison between this method and other methods for combining the various forms of information, including a median error-based metric or an energy-based formulation.
For each model in the set of models, i.e., m ∈ M, a likelihood metric,L m , is evaluated from the combined error metric: Note, the initial value of the combined error metric, CE, as defined in equation (30) is set equal to one to avoid division by zero in the calculation of the likelihood metric.
VI. SEGMENTATION A demonstration may contain a single constraint or multiple constraints of various types. Additionally, an individual constraint may be visited multiple times during a single demonstration. Finally, free space motion may occur between constraint interactions or different constraint interactions may occur consecutively, with no intermediate free-space motion.
In order to apply regression and model selection to each constraint interaction individually, the associated samples of the constraint must be determined from the demonstration. While user-specified segmentation is possible, it is desirable to make the process automatic to avoid the need for user intervention to define constraint boundaries for segmentation.
Prior work has explored the segmentation problem. One approach [15] is to fit each constraint type over a small sliding window. Similar constraint fits are then clustered over the entire demonstration. While effective, this approach is computationally expensive and limited to simple constraint models. It also requires significant tuning of heuristics by the user to achieve reliable segmentation. In another method, [23], change point detection is used to determine constraint locations and can detect transitions between constraints without free-space motion, but this method is limited to simple models such as lines and arcs.
In our method, we use an extension to the basic idea of force thresholding. If a measured force exceeds a specified threshold (e.g. twice the measurement noise of the system), then a constraint may be present. The result is a Boolean list of contiguous samples indicating possible constrained motion segments and free space motion segments. One limitation of this simple segmentation method is that free space motion must occur between each constraint interaction. While our experimental demonstrations are designed to meet this assumption, other methods that identify transitions between contiguous constraints or that incorporate multiple constraints within a single model could be incorporated in the future.
Since measured forces can occur due to other effects including gravitational force or forces induced from accelerating motion, force thresholding is susceptible to false positive segments. To compensate for this we extend force thresholding to include a constraint null hypothesis.

A. Constraint Null Hypothesis
The goal of the constraint null hypothesis is to remove false positive constraint fits. The fitting and model selection procedures described in sections IV and V determine the best model that matches the data for a particular segment. For each of these segments, we evaluate whether the constraint should be kept or discarded (i.e. is it a false positive). An error threshold for each error metric (position, orientation, force and moment) is applied to each sample in the segment and contributes to cumulative tallies V r , V q , V f and V n . A tally is awarded when the error is more than the threshold. The thresholds are set above the system's measurement noise. If the percentage of cumulative tallies for any metric is greater than an acceptable maximum percentage (e.g. 75% in our implementation), the segment is discarded. Additionally, any segment consisting of only a few samples will easily fit a given model, but does not lend to confidence that the data is actually constrained. For example, if the segment is only two points, the prismatic relaxed model will always fit with zero error. To avoid this case, we discard segments of very short duration (in our implementation, less than or equal to five samples).

VII. EXPERIMENTAL EVALUATION AND DISCUSSION
The constraint inference approach is evaluated in three experiments. In the first two experiments, we desire to assess the situations in which wrench information improves constraint fitting of unknown objects. To do so, we compare a method using pose and wrench measurements to an approach that uses only pose measurements. These two experiments include (1) an evaluation of model fitting (Section VII-B) for demonstrations containing a single constraint and (2) an evaluation of model selection (Section VII-D) for demonstrations containing a single constraint. The third experiment seeks to evaluate the complete constraint inference method on a situated demonstration containing multiple differing constraints.

A. Experimental setup
We desired to show that our methods can reliably recognize and parameterize constraints through a variety of demonstrator strategies. In all three experiments, data was collected from 9 participants with no prior knowledge of or prior experience with the research recruited from the university student population. The study took 60 minutes and all participants received $12 as compensation. To acquire the resulting pose and wrench measurements that occur during constraint interactions, subjects used custom instrumented tongs [6] equipped with force-torque sensors (ATI mini40) and motion capture markers (Opti-track Flex 13) to perform demonstrations. The instrumented tongs, shown in Fig. 6, provide measured position, orientation, force, and moment trajectories sampled at 120 Hz (after appropriate resampling and filtering). While our method extends to other input methods capable of measuring interaction wrenches, instrumented tongs provides a familiar interface for human demonstrators.
For the first two experiments, where the model fitting and model selection performance are evaluated, an experimental test bed, incorporating various physical constraints was constructed (see Fig. 7). Each constraint in the test bed has a designated grasp location that participants use to interact with the constraint.
In all demonstrations except point-on-plane, point on line, and the situated demonstration, users interact with a nonrigid spherical interface. For constraint models that do not incorporate orientation constraints, such as prismatic relaxed, planar relaxed, and axial relaxed constraint, the measured pose information is taken directly from the instrumented tong motion capture markers. For constraint models where orientation of the constrained body is enforced, such as point-onplane, point-on-line, prismatic, axial, and planar constraints, the pose information is taken from motion capture makers attached directly to the manipulated object. While our method can fit rigid interactions using the pose of the instrumented tongs, users would allow rotation when interfacing with the spherical interface, and using separate markers attached to the object allowed for rigid and relaxed data to be captured from a single demonstration without having to instruct users to perform a separate set of demonstrations where objects were held rigidly. In cases where the object markers are used, the applied forces and moments measured using the instrumented tongs are transformed to the object coordinate frame by relating the object and instrumented tongs motion capture markers.

B. Evaluation of Model Fitting
The first experiment seeks to evaluate the performance of constraint model fitting (Section IV) for demonstrations containing a single constraint. We compare the performance of a method using pose and wrench information to our previous method [14], [15], which uses only pose measurements.
Specifically, we evaluate fit accuracy as a function of demonstration duration. In the experiments, participants were instructed to interact with each physical constraint on the test bed. Two 10 second demonstrations were recorded per constraint. To simulate varying lengths of constraint interactions, contiguous segments are randomly chosen from across the entire demonstration and fit using the method described in IV. For comparison, the data was also fit to constraint models using a method that considered pose information only. In this case, the method is identical to that described in Section IV except that the maximum likelihood estimate objective function relies on the kinematic error objective given in equation (19) only (i.e., does not include the wrench error objective function from equation (23)).
To compare the methods, a fit error based on the position error metric described in equation (26) is computed for both methods. While a variety of error metrics are possible, we chose a position-based method to allow comparison across constraint models and as a method that can be related to unexpected applied force through a hybrid controller. The fits are evaluated against either the entire recorded demonstration or for point-on-line and point-on-plane, an independent demonstration performed by the experimenter (since the fitting requires a significant amount of the original 10 second demonstration). Fig. 8 shows a regression analysis for constraints with large number of degrees of freedom. Fit accuracy is evaluated as a function of contiguous samples (sampled at 120 Hz). For models with high degrees of freedom (point-on-plane -5 DOF, planar relaxed -4 DOF, planar -3 DOF, and point-on-line -4 DOF) considerably fewer samples of combined kinematic and wrench data are required to fit models than for kinematic (i.e., pose) data alone. Wrench measurements provide additional information that compensates for degenerate kinematic information allowing a constraint to be fit with fewer samples. Fig. 9 shows a comparison between fitting the planar relaxed constraint model with pose and wrench measurements compared to fitting with only pose measurements for a particular participant's demonstration. Short demonstrations on the planar relaxed constraint may consist of primarily linear motion which leads to degenerate planes when fitting with only kinematic data. Motion in a straight line will not completely determine the orientation of the plane as many different planes may fit the same line.
For models with fewer degrees of freedom such as the prismatic, axial rotation, axial relaxed, and planar constraints, the wrench method yields similar results to the kinematiconly fitting method. This is expected since high numbers of kinematic constraints lead to many directions of admissible forces and moments which limits the utility in fitting. For example, in the prismatic model, there is only a single degree of freedom that is not constrained. However, this is the direction of motion which is also the direction in which friction occurs. Since we assume that forces in this direction could be friction, they are removed. The remaining two directions are acceptable reaction forces. As a result, there is no additional information added from including wrench measurements in fitting for this model. Fig. 10 shows a regression analysis of number of samples verses fit error for the constraints with a small number of degrees of freedom. The axial relaxed and axial rotation models have high nonlinearity and sometimes the fits are incorrect due to convergence to a local minimum. This explains the large standard deviation for both models even when using a large number of samples.
In this experimental setup, the orientation of the force frame is derived from motion capture orientation measurements. As a result, the force and moment errors tend to be higher based on the propagated orientation error. This can be seen in the increased noise floor for the wrench fitting method in the regression plots (i.e., the wrench method converges to a higher error than the kinematic method). While this is a product of the experimental setup, it demonstrates that it may still be beneficial to consider using kinematic fitting for constraints where the force and moments add little information (e.g., prismatic constraint) to avoid cases when the forces and moments have a significant noise floor and to simplify the dimensionality of the non-linear optimization.
The experiments show that wrench measurements improve fitting of models with high degrees of freedom over short demonstration duration. They help in situations when kinematic information is insufficient to completely define the constraint model. For constraint models with high degrees of freedom, we recommend that wrench measurements are incorporated in the fitting process. For models with low degrees of freedom, wrench measurements can be omitted to decrease computation time and reduce the complexity of the optimization process.

C. Evaluation of Model Selection
The second experiment seeks to evaluate the performance of constraint model selection (Section V) for demonstrations containing a single constraint and compare the performance of a method using both pose and wrench measurements to an approach that uses only pose measurements. For comparison, we also include a penalized kinematic method [24] to address limitations with a kinematic-only method, where demonstra-

Kinematic and wrench fitting
Kinematic fitting Samples used to fit model Forces Demonstration Fig. 9: An example demonstration of the planar relaxed constraint from Demonstrator 6. Using a small portion in the demonstration (cyan) of 0.2 seconds fitting with only kinematic data (red plane) does not produce the correct model. Including reaction wrenches in the analysis fits a better plane (green) to the demonstration. tion data may be consistent with a number of different kinematic models and thus result in poor selection accuracy. The penalized likelihood metric is defined as follows: where DOF refers to the kinematic degrees of freedom of a model and χ is the number of model parameters (Corresponding values for the constraint models used in this paper are provided in Appendix A. Weighted parameters β and γ are fit using eighty training demonstrations (10 per constraint model).
In our experiments, the demonstrations are split into 3.33 second segments (sufficient samples for fitting). Since the demonstrations are sufficiently long, the constraint parameters for all demonstrations are fit with kinematic data to ensure that the differing methods use the same constraint fit parameters. Since model selection is requisite on the constraint being properly fit, fits that do not match the performed constraint are manually discarded. The results appear in Fig. 11.
The results shown in Fig. 11 were, in part, evaluated using the error tallying method described in equation (30) which, in turn, requires that we set the thresholds for each of the error metrics. The metric values are set based on an estimate of the data collection accuracy. The force and moment error represent the component orthogonal to the permissible force and moment directions. Since the errors are normalized with respect to the measured force and moment, the formulation is equivalent to evaluating the sine of the angle between the measured and normal vectors. In our experimental evaluation, the error for the force and moment is set to the same value as the orientation error (2 degrees). The list of parameter values is provided in Table I   Our results demonstrate the value of including wrenches in constraint model selection. As seen in Figure 11, the method including wrench information in model selection greatly outperforms equivalent kinematic and penalized-kinematic voting methods. In the kinematic-only tallying method, incorrect classifications can be attributed to model generality. For example, the axial relaxed constraint is always identified as planar relaxed. When only kinematic measurements are considered, this is a likely case. There is always a plane that can describe an axial motion and moreover any small errors in the position might still be consistent with the plane model, but not necessarily with the axial model. Two other models, (planar constraint and prismatic constraint), are consistently misidentified as their relaxed versions. As a reminder, the relaxed version of the constraint does not enforce a rigid orientation. Similar to the previous case, any small errors when the orientation is rigid favor the relaxed model in the selection process. The kinematic method also misidentifies the rigid axial constraint as a point on line constraint. In this case, both models do not receive any tallies and as a result, the selection is arbitrary based on the order in which the models are selected. Penalization of the kinematic method increases classification accuracy from 0.52 to 0.73 correcting for many of the issues with the general model. However, even with correction, the axial relaxed model is still identified as a planar relaxed constraint.
Use of the combined error greatly improves the classification accuracy (0.87). The only model that is consistently misidentified is the prismatic relaxed constraint. In most cases, the identified model is the rigid version of the constraint. The relaxed model does not enforce an orientation which consequently means there can be no generated moments. When a participant interacts with the tongs, the grip can slip leading to changes in orientation, but for the most part, the generated moments and rigidity may be more consistent with the rigid model. As such, these cases are not necessarily incorrect classifications but instead represent cases of demonstrations that lie between the two constraint models.

D. Situated Demonstration
We evaluate our system on a proof-of-concept situated task. The task involves preparing an espresso using the instrumented tongs as an input device. The experimental setup for the espresso preparation task in shown in Fig. 1. 1) The demonstrator first slides an espresso cup under the espresso machine spout (planar-relaxed constraint).
2) The demonstrator then uses the instrumented tongs to pull out a drawer (prismatic-relaxed constraint) containing a single coffee pod. 3) Using the tongs the demonstrator carries the pod to the espresso machine and places the pod inside. 4) Finally the demonstrator uses the tongs to close the lever (axial-relaxed constraint) of the espresso machine.
Using the system described in this paper, the demonstration is parsed for the three possible constraint models: planar relaxed, prismatic relaxed, and axial relaxed constraints. The three relaxed models are used as the instrumented tongs have slip directions and do not enforce a rigid orientation when grasping the constrained objects. The two 1-DOF models, prismatic relaxed and axial relaxed models, are fit using the kinematic method, to avoid the increased noise floor of the experimental setup.
The results of the constraint inference are shown in Fig. 12. The system correctly identifies 37 out of the 48 constraints in 18 demonstrations (2 per participant). Note, data was removed in instances where participants did not correctly interact with a given constraint (e.g. participants lifted the cup rather than sliding it on the table). Fig. 12: Confusion matrix of the end-to-end system on 18 espresso making demonstrations (9 participants, 2 trials, 3 potential constraints per task). In 6 demonstrations, the participant lifted the cup rather than sliding it, so there was no planar constraint to be detected (*).
The espresso setup evaluates the method on a realistic task where the constraints may not be as precise as the constraints in the earlier experiments (using the experimental test bed shown in Fig. 7). For example, the drawer used for linear motion is plastic and has 1.5 cm of play in its constrained motion. Additionally, the espresso machine lever (axial relaxed constraint) can be difficult to grasp leading to some demonstrations that do not resemble axial motion. As expected, a few constraint interactions were misclassified by model selection or rejected by the null hypothesis. In spite of the imprecise constraints, the system correctly infers a majority of the constraints.
The null hypothesis was successful in rejecting nonconstrained motion that exceeded the force threshold, such as when participants hold the coffee pod and accelerate towards the espresso maker. In the 18 demonstrations, the null hypothesis rejected 20 extraneous segments. Fig. 1 shows the results for one of the participants where the method is successful. Our approach correctly selected the model 77% of the time. Correctly inferred constraints were all correctly fit (as shown in Fig 1). Constraints were identified using a single demonstration in which the demonstrator performed them naturally with minimal instruction.

VIII. CONCLUSION
The limitations of our method suggest future areas of extension. In our methods, we assume free space interaction occurs between each of the constrained segments. While we have not explored how often this assumption is valid, other work has developed methods for detecting changes between consecutive constraints.
In this work, we focus on methods for constraint inference, however, we do not explore task replication by a robot. While our method provides a general set of tools to recognize constraints and their associated parameters, it is unanswered whether these models appropriately define a task such that a robot could perform similar actions.
Additionally, this work does not discuss human procedures that may be identified and leveraged during replay. We also do not explore remapping of constrained motions between a general input method and a robot which may have different kinematic restrictions. Our method describes when constraints occur and their parameters for a given demonstration. Future work will investigate editing these representations, for example to modify a motion on a constraint.
In summary, we present a framework that is capable of recognizing geometric constraints in a demonstration. Specifically, the method includes the ability to segment sections of constrained motion, fit models using both kinematic and wrench measurements, and select the best candidate model for each section. We show that wrench measurements enable estimation of constraints with high degrees of freedom from fewer samples compared to using only kinematic measurements. We demonstrate the use of wrench measurements in improving model selection classification accuracy compared to a similar method using only kinematic measurements. We demonstrate that model selection can be further improved by incorporating model generality through the degrees of freedom and number of parameters of candidate constraint models. We evaluate our end-to-end constraint inference method on a situated task of preparing an espresso which correctly identifies 77% of constraints without any user input.

APPENDIX A MATHEMATICAL MODELS OF GEOMETRIC CONSTRAINTS
This section describes the mathematical model development for the various constraint types evaluated in the paper (see Fig.  3). The mathematical model for the point-on-plane constraint is provided in section III-A1.
For all constraint types, our mathematical model considers a 6-degree of freedom rigid body of negligible inertial properties located in space through 3 translational coordinates r ∈ R 3 and rotational coordinates represented either by a unit quaternion q ∈ Spin(3) or an orthogonal rotational matrix A(q) ∈ SO(3). Together (r, q) form the body coordinates p. Each constraint type has a set of parameters, α α α, that parameterize the geometry of the constraint.

A. Planar constraint
The planar constraint is exemplified by an eraser moving against a whiteboard. The rigid body interacting with the plane (e.g. eraser) can only rotate about a vector perpendicular to the plane, and all points within the rigid body translate parallel to this plane.
A plane may be specified by applying a general displacement (i.e. translation and rotation) transformation of the x-y plane which involves: 1) translating the x-y coordinate plane along the z-axis 2) rotating the translated plane about the origin. We represent the rotation transformation using two exponential coordinates w = [w x , w y , 0] ∈ R 2 corresponding to ew ∈ SO(3), the exponential map, which is equivalent to a rotation matrix with an axis of rotation in the x-y plane. Rodrigues' rotation formula [17] is used to compute ew.w is the skew symmetric matrix similar to equation (62) in Appendix B. The third term of w is zero because rotations about the z -axis (perpendicular to the plane) do not alter the plane's geometry. The translation is represented by d ∈ R. The normal vector on this plane is represented by ew[0 0 1] T and the shifted origin of the x-y plane is represented by ew[0 0 d] T .
The origin of the local coordinate frame on the body is coincident with the plane. The constraint equations for motion on a plane is given as: A unit vectort is defined to force the rigid body perpendicular to the plane: Unity oft is enforced by equation (36). The parameters of this constraint are α α α = (t, d, w x , w y ). The rigid body has 3 degrees of freedom. Friction Model: The friction force is in the direction of linear velocity and the friction moment is in the direction of angular velocity.
where F and N are the measured force and moment, respectively. The measured linear and angular velocity are v and ω ω ω, respectively.

B. Planar relaxed constraint
The planar relaxed constraint is represented by equation (33). The friction model is the same as the planar constraint. C. Prismatic constraint The prismatic constraint represents translational motion in one direction. It is similar to pulling out a drawer. All points on the rigid body translate identically. This constraint requires a representation of the axis of translation. Similar to the plane, the axis of translation can be generated by applying a general displacement (i.e. translation and rotation) transformation of the z coordinate axis which is equivalent to: 1) translating the z axis in the x-y plane 2) rotating the translated axis about the origin. This is represented by two exponential coordinates w = [w x , w y , 0] ∈ R 2 and two translational coordinates d x , d y ∈ R. The third term in w is zero because rotations about the zaxis produce a line that could be produced by an alternative translation motion. This defines the axis in the global reference frame. The tangent to this axis is ew where F is the measured force and v is the linear velocity.

D. Prismatic relaxed constraint
The prismatic relaxed constraint is obtained by only using equations (39) and (40). The friction model is given by equations (46) and (38).

E. Point-on-line constraint
The point-on-line constraint represents a point on a rigid body constrained to a line in space. It is similar to a pen moving against a straight edge.
Consider a point s * ∈ R 3 in the global reference frame defined as a fixed point on the rigid body. s is a vector directed from the origin of the local reference frame of the body to the rigidly fixed point s * in the global reference frame. The local reference frame counterpart of s is − s : where A(q) is the orthogonal rotation matrix equivalent to the rotation represented by q. s * on the rigid body is coincident with a line represented by parameters (d x , d y , w x , w y ): Friction Model: The friction models are identical to the friction models for the point-on-plane constraint described in section III-A1.

F. Axial rotation constraint
The axial rotation constraint is similar to a door knob or a hinged door, all points on the rigid body rotate about an axis and translations are not permitted.
Consider a point on the rigid body s * that is on the axis of rotation and in the plane perpendicular to the axis containing the coordinate frame origin. A rigid point on the axis of rotation defined as (d x , d y , d z ) ∈ R 3 constrains the point s * . The vectors is perpendicular to this axis. The rigid body must not rotate about vectors so unit vectort is introduced to enforce this condition. The constraint equations are (36), (45) and the following: Constraint parameters are α α α = (t,s, d x , d y , d z , w x , w y ). This constraint has 1 degree of freedom. Friction Model: The friction wrenches and reaction wrenches are coupled in this model. The principle of virtual work enables the computation of reaction forces and reaction wrenches from the corresponding measured quantities. Let's assume all measured forces are reaction forces. This implies that friction forces are zero: The principle of virtual work requires that wrenches are orthogonal to twists as per Equation 7 (substituting virtual displacements with velocities). The residual of equation 7 provides the corresponding friction work W µ : The friction work comes from the friction moment about the axis of rotation. The friction moment is directed about the axis of rotation.

G. Axial relaxed constraint
The axial relaxed constraint also has parameters (d x , d y , d z , w x , w y ) to configure the axis of rotation and the center of rotation. The radius l ∈ R is included to fully define the constraint.  Table II shows the degrees of freedom and number of constraint parameters for each of the models considered in this paper. These values are used to construct the penalized likelihood method used for comparison in section VII-C.  APPENDIX B RELATIONSHIP BETWEEN π AND Q Evaluating the partial derivatives of the constraint equations, Φ, is required to determine admissible reaction forces and moments for a given constraint. The partial derivative with respect to rotation uses the virtual rotation variable, π π π. This section describes the derivation of π π π in terms of the known orientation, q.
where the rotation is described by quaternion q = e 0 e T T and where e 0 = q 0 and e = [q 1 q 2 q 3 ] T . A is the orthogonal rotation matrix and G is a matrix constant.


Fast Prediction of a Worker’s Reaching Motion Without a Skeleton Model (F-PREMO)

This paper proposes a fast and highly accurate prediction method for the reaching motions performed by a worker in cooperative work with a robot, called fast prediction of reaching motion (F-PREMO). Cooperative work is permitted by a 2011 ISO revision under the condition that risk assessment has been performed. To increase production effectivity, it is essential to predict the worker’s movement and control the robot based on the prediction. This paper focuses on the movement of a worker’s hand, called the reaching motion, which is relevant to many types of assembling tasks. The most common existing methods require attaching markers to the workers or installing three-dimensional (3D) sensors in front of the workers since these methods require the real-time estimation of skeleton models of the workers. These requirements lead to difficulties in introducing prediction methods at production sites. To solve this problem, we propose a prediction method for the reaching motion that neither requires the attachment of markers nor limits the placement of the 3D sensor. The proposed method first computes a feature vector of the reaching motion and then performs the prediction using random forest with the computed feature vector as input. Experimental results show that the proposed method can predict the reaching motion from the initial 50% of the movement of the worker with more than 80% accuracy in less than 5.2 [ms].

I. INTRODUCTION
There is an increasing need for cooperative work between humans and robots in fields such as object transportation [1], [2], nursing care [3], [4], and product assembly [5], [6]. Particularly in the industrial field, many collaborative industrial robots have been developed and operated without any separation between the workers and industrial robots, primarily due to the following reasons: 1) The safety fences can be removed after a risk assessment has been performed, as defined in ISO10218-1:2011 and ISO10218-2:2011 (revised in 2011); 2) Cooperative work has great potential for improving production efficiency by decreasing production time, shortening the production line, and decreasing the number of workers.
The associate editor coordinating the review of this manuscript and approving it for publication was Tao Wang .
For these two reasons, a number of robots have been developed for cooperative work, including CR-35iA [7], duAro [8], NEXTAGE [9], YuMi [10], LBRiiWa [11], and Baxter [12]. For example, the FANUC corporation developed an industrial robot for cooperative work, called CR-35iA. This robot has the following three safety functions [13]: 1) Contact stop function, which is activated on the physical impact between the robot and worker; 2) Push to escape function, under which the robot may be shoved in any direction by a human operator; and 3) Shock absorption function, which is achieved by covering the robot's surface with cushioned materials. ABB has also produced a collaborative industrial robot called YuMi [10]. YuMi can execute emergency stops within less than 10 [ms] after detecting an unexpected impact, and the surface of the robot is covered with soft pads for absorbing the physical impacts. In addition, the positions of YuMi's joints and the length of each joint are carefully designed such that there is no gap where the worker's fingers can get stuck.
These robots ensure worker safety by utilizing emergency stop functions when collisions are detected and by computing trajectories for collision avoidance based on the distance between the robot and the worker [14]- [16]. In other words, these safety functions are based on the current state of the robot and the worker. In addition to these functions, predicting the worker's movement can greatly improve production efficiency by decreasing the number of emergency stops, increasing the running speed of the robot, and computing more efficient trajectories for collision avoidance [17].
The prediction of worker movements at production sites is categorized into the following two types. The first is the prediction of the movement of the worker's center of mass [18], [19], while the second is the prediction of the motion of the worker's arm, which is referred to as the reaching motion [29]- [31]. The former is mainly treated as a problem of predicting the movement of a mass point. In contrast, predicting the reaching motion is more difficult since the dynamics of motion of a human arm are highly nonlinear [32]. However, in many assembly tasks, the worker's arm moves with nearly no change in the worker's center of mass.
Therefore, this paper focuses on predicting the reaching motion of a worker. A number of prediction methods for reaching motion have been proposed. A prediction method that uses a Bayes classifier and assumes that the positions of each joint are recognized and measured is reported in [31]. In their method, the reaching motion is considered to be divided into two layers: the upper layer defines the type of task for the reaching motion and the lower layer provides the motion. The method proposed herein predicts the type of task and motion, and subsequently, integrates each prediction. Their paper reports that a prediction accuracy of greater than 70% is achieved based on 1/3 of the trajectory of the reaching motion.
With technological advances on three-dimensional measurement [20], [21], a lot of robotic tasks have been achieved with 3D sensors, such as, positioning [22], [23], pose estimation [24]- [26], picking [27], [28], and measurement of worker's movement. Researches [29] and [30] also employed motion-capture systems to detect the arms and utilized the Gaussian Mixture Model (GMM) algorithms to predict the reaching motion of the arm. Their studies focused on learning models that describe how humans cooperate, which can be applied to cooperative human-robot work. Other similar techniques used to investigate human motion have been studied by using Hidden Markov Models (HMMs) [33].
In these models, the positions of the worker's joints, particularly the arm joints, are measured using a motion-capture system, which requires (1) attaching markers to the worker and (2) placing at least three cameras. These requirements increase the cost of installing a cooperative production system and place limitations on the working environment.
Alternative methods for predicting the reaching motion without a motion-capture system have been proposed using a skeleton model of the worker.
For example, a prediction method for the reaching motion based on gesture recognition was proposed in [34]. In this method, the various types of worker movements are registered in a database in an offline process. The movements are then categorized into classes, and one GMM is assigned to each class. The volume swept by the worker is also computed for each class. Next, in an online process, the current motion is first categorized based on the recognized gesture, and the time series of the volume swept by the worker is predicted. This method achieved prediction accuracies of 50% and 92% based on measured data of 43% and 80% initial trajectories, respectively.
Ravichandar et al. proposed a prediction method for the trajectory of reaching motion and reaching point based on the dynamics of a human arm model using a neural network (NN) [32]. In their experiment, a skeleton model of the worker was tracked in real-time with a Kinect sensor. The prediction accuracy and prediction time of this algorithm were 87% (seven correct predictions out of eight trials) and 50 [ms], respectively.
Jiang et al. [35] proposed a method that identifies the skeleton model and predicts the reaching motion by using the identified skeleton model. They represented the skeleton model as a mapping between high-and low-dimensional representations in the skeleton model by using latent conditional random fields to model the spatial and temporal contexts of human activities. The method of Jiang et al. took 11.2 [s] to compute 10 [s] of the reaching motions.
The above methods estimate the joint positions of the worker by using the estimated skeleton model in real-time. However, to develop the skeleton model, it is necessary to place a 3D sensor in front of the worker in order to capture the upper body. In other words, these methods place limitations on the placement of the 3D sensor.
To overcome these limitations, we propose a fast and highly accurate prediction method, called fast prediction of reaching motion (F-PREMO) for the prediction of reaching motion without the need to attach markers to the worker or place a 3D sensor in front of the worker. The proposed method divides the common working area of the worker and robot into different regions and predicts which region the worker's hand will reach into. The proposed method mainly comprises the three following processes. First, the method extracts a set of 3D points comprising of the worker, which is called the ''worker's cluster'' in this paper, from the obtained 3D point cloud at each time step. Next, the method calculates a feature vector of the reaching motion from the time series composed of the worker's clusters. Finally, the proposed method predicts the region into which the worker's hand will reach by using a random forest. We confirmed the performance of the proposed method using an assumed assembling task. We conducted an experiment using a Microsoft Kinect v2 [36] as the 3D sensor; the sensor was mounted above the worker rather than in front of the worker. The experimental results showed that the proposed method could accurately predict the reaching motion based on 50% initial data for the reaching motion. In addition, the average computational time for the prediction was 5.2 [ms]. These results show that VOLUME 8, 2020 FIGURE 1. Situation for cooperative work considered in this paper. The working space consists of three types of areas: the robot's working area, common working area, and the worker's working area. In the robot's and worker's working areas, only a robot and worker can be located, respectively. The common working area is divided into several regions, each of which is assigned a label. A 3D sensor is located at a position where it can observe the common working area and movements of the worker.
the proposed method can quickly and accurately predict the reaching motion without requiring marker attachments or 3D sensors in front of the workers.
The remainder of the paper is organized as follows. Section II describes the problem considered in this paper: predicting the reaching motion of a human doing cooperative work with a robot. Section III describes the processes for computing the feature vector showing the worker's reaching motion from 3D point cloud data obtained by a 3D sensor. Section IV describes the semi-automatic process for creating training data and the process to construct a predictor using a random forest. Section V experimentally examines the performance of F-PREMO, including prediction accuracy and computation time. Section VI concludes this paper.
The following notations are used in this paper. A sphere with its center located at p ∈ R 3 and radius r ∈ R is denoted by S(p, r). Elements of set A are represented by a i (i = 1, 2, · · · ). The symbol |A| is defined as the number of elements of a set A. Using the above notations, we can write (1)

II. PROBLEM STATEMENT
As described in the previous section, this paper aims to predict a human's reaching motion during cooperative work between the human and the robot. In cooperative work, there is a chance of collision in the common working area where the robot and worker co-exist. To address the above problem, we consider the working situation shown in Fig.1. This paper assumes three types of areas: the robot working area, common working area, and worker's working area. The common working area is divided into several regions, each of which is given a label. We will FIGURE 2. Prediction process from 3D point cloud data to prediction in the proposed method. Based on the 3D point cloud data, the method categorizes the point cloud and identifies 3D points representing a worker, as shown in Sec. III-B. We refer to the 3D points as a worker's cluster. The proposed method then computes a feature vector using the time-series data of the worker's clusters, as shown in Sec. III-C. Finally, the method predicts the reaching motion from the current time t to t + T by using a random forest, as shown in Sec. IV.
propose a method that predicts the area that the worker's arm is reaching toward.
In addition, the proposed method does not require attaching markers to the worker or placing the 3D sensor in front of the worker. The only requirement concerning the placement of the 3D sensor is to mount it such that it can observe the worker movements in the common working area and in the worker's working area.
We can summarize the problem described above as follows: Problem 1: There exists a common working area for a worker and a robot, which is divided into n regions represented by 1 , 2 , · · · , n . The time-series data of the 3D point cloud in the common working and worker's working areas are assumed to be given from the initial time 0 to the current time t, and are denoted by P(0), P(1), · · · , P(t). The region located closest to the robot's working area S robot and where the worker's hand will reach from the current time t to t + T is determined as where d(X , Y ) denotes the distance function for the regions X and Y .Î(t + 1 : t + T ) is the predicted index set of regions toward which the worker's hand will reach from time t + 1 to t + T , and is defined by where E(J 1 , J 2 , · · · , J n ) is a mapping from the multi-set {J 1 , J 2 , · · · , J n } to a unique set in which each element has a multiplicity of 1. The symbol V th represents a threshold for volume. The symbolV worker represents the predicted area occupied by the worker at time τ and X ∩ Y volume denotes the volume of the intersection between regions X and Y.
To solve Problems 1 and 2, we first propose a method to extract the worker's cluster from the obtained 3D point cloud data; this method is a solution to Problem 1, shown in Sec. III-B. We then propose a process to compute a feature vector that represents the reaching motion, as described in Sec. III-C. Finally, we present a method to estimate the region i * that satisfies (2) by using the time-series data of the feature vectors; This method is a solution to Problem 2. . Process for computing a feature vector for the worker's reaching motion. This computation consists of four steps. In the first step, the worker's cluster is determined from point clouds obtained by a 3D sensor. In the second step, the 3D space is divided into voxels, and each voxel is binarized such that whether the number of points for the worker's cluster located in each voxel exceeds a threshold or not. After this process, the binary vector f (t ) is obtained in the third step. In the fourth step, the feature vector of the worker's reaching motion F (t ) is computed from the time series of the binary vectors f (t ), f (t − 1), · · · .

III. COMPUTATION OF FEATURE VECTOR
This section describes the process of computing a feature vector of the worker's reaching motion. The process mainly consists of four steps, as shown in Fig.3.
In the first step, the worker's cluster is extracted from 3D point cloud data. In the second step, the working area is divided into voxels and each voxel is binarized to indicate the presence of the worker. Subsequently, the binary vector f (t) is obtained in the third step. Finally, in the fourth step, a feature vector F(t) is computed based on the time series of binary vectors f (t), f (t − 1), · · · . Section III-A will explain preparation needed to be performed for the process of computing the feature vector. The first step is described in sec. III-B and the rest of the steps are shown in sec. III-C.

A. PREPARATION
First, it is necessary to install a 3D sensor in a place where it can observe the common working area and the worker's working area. It is not necessary for the 3D sensor's observation area to cover the entirety of the worker's working area. In addition, the 3D sensor is assumed to be able to obtain depth images of the scene. Thus, each 3D point obtained by the 3D sensor will have a corresponding 2D representation (u, v) in the camera's view, which can be represented by where (x, y, z) and s are the obtained 3D point's coordinates and a scale parameter, respectively. A number of models have been proposed for the function g [37]. The simplest one is the pin-hole camera model, which can be represented by where K is the intrinsic matrix and includes the focal length of the camera. Second, a rigid body conversion matrix T S M to transform the manipulator's coordinate system to the 3D sensor's coordinate system can be obtained by offline calibration. Next, the 3D shape of the manipulator is approximated by a primitive shape χ (e.g., a cuboid or a sphere) [14], and a function is identified, where V M and p joint i , i = 1, 2, · · · , J represent the approximated occupied space of the manipulator and each joint position of the manipulator, respectively. As a final phase in the preparation, the 3D sensor obtains a point cloud data for background where the manipulator is set to be in an initial pose, and the worker is not present.

B. RECOGNITION OF WORKER BASED ON 3D POINT CLOUD DATA
This section describes the process for extracting the worker's cluster of 3D point cloud data from the obtained 3D point cloud data at each time point.
First, the 3D points consisting of the manipulator are recognized in the 3D point cloud data as follows. Here, the 3D point cloud data are denoted by P(t) := {p 1 (t), p 2 (t), · · · , p n t (t)}. For the extraction, each joint position of the manipulator q M i , i = 1, 2, · · · , J in the robot coordinate system is computed by encoders from the angles of the joints. The joint positions are converted into the 3D sensor's coordinate system by From the computed joint positions, we can specify the space occupied by the manipulator as The 3D points corresponding to the manipulator are recognized by Next, we perform the background subtraction for the point cloud and extract the changed points to satisfy where p (u,v) (t) ∈ R 3 represents the 3D point corresponding to the 2D representation (u, v) and ψ z (p) is a function that returns the z coordinate of a 3D point p. Here, the set of points satisfying (11) is defined by P bs (t) := {p (u,v) (t) | satisfies eq.(11)}.

VOLUME 8, 2020
The manipulator's points are then removed from P bs (t); that is,P bs (t) := P bs (t)\P M (t).
The clustering is performed for pointsP bs (t) using either the 3D representation of the points [38] or the projected 2D representation of the points [39]: The clustered points are then obtained. 1 The clusters inP bs (t) with more than worker points are considered to be the worker; that is, where worker is the user-defined threshold, which should be set depending on the worker's volume.
We write the above process for extracting the worker's cluster of 3D point cloud data from P(t) as P worker (t) = ξ (P(t), P(0)), where P(0) represents the 3D point cloud obtained as the background. The function ξ in (15) is a solution to Problem 1.

C. FEATURE VECTOR OF THE WORKER's REACHING MOTION
This section describes the process of computing a feature vector of the worker's movement from the obtained worker's cluster derived in the previous section. The reaching motion is predicted using a random forest [40] with the feature vector as input.
First, we assume that the worker's cluster of 3D point cloud data is given from the initial to current time P worker (τ ), τ ∈ {0, 1, · · · , t} by using the method shown in sec. III-B.
The 3D space is then divided into voxels with length L voxel on each side. Each voxel is turned into a binary state to indicate the presence of the worker inside a voxel. If at least one point from P worker (t) is mapped to the voxel, the voxel takes the value 1; otherwise, the voxel takes the value 0. After this process, the 3D voxel space is mapped into a 1D space, creating a binary vector denoted by f (τ ) ∈ {0, 1} n f , τ = t, t − 1, · · · , as shown in Fig.3. Now, we are ready to define the feature vector as where κ 0 and κ 1 are user-defined constants for time delay. The function δ represents the time change for a worker's pose and is defined by 1 We have used an approach similar to that of [39] in sec. V.

IV. PREDICTION OF REACHING MOTION WITH FEATURE VECTOR
This section explains the prediction process by using random forest as a classifier. Before use, the random forest is trained on the data pairs of the feature vectors and region labels. These training data are semi-automatically created from time 0 to T train using the following process. The training data, which are constructed of pairs of the feature vector F(t) and the region label (t), are created in real-time without postprocessing. To create the training data, the worker only needs to indicate the starting time t i for the i-th reaching motion. The end time for each reaching motion is automatically identified as follows. Once the worker's hand is detected inside the region i in the common working area at time t reach i , a timer will run for the duration s timer . If the worker's hand has been inside the region i during the whole duration of s timer , the training dataset D i for the i-th reaching motion is constructed by (18) where T i represents prediction horizon, which is defined as t reach i − t i + s timer . The collection of n train reaching motion samples is combined as Not all feature vectors from time 0 to T train are included in D predict . The feature vectors that are not included in D predict (i.e., the worker is not performing a reaching motion toward the common working area) are denoted by F(t no i ) and collected in D no := (F(t no 1 ), −1), (F(t no 2 ), −1), · · · , (F(t no m no ), −1), (20) where the label -1 indicates no reaching motion toward the common working area. Here, note that {t no 1 , t no 2 , · · · , t no m no } = {0, 1, · · · , T train }\{t 1 , t 2 , · · · , t n train }.
Finally, we combine all training datasets generated by the above process into one training dataset: The random forest classifier consisting of n tree decision trees is then built using the training dataset. Each decision tree in the random forest is trained on randomly chosen data pairs (F(t), (t)) from D, where F(t) is the input vector to a decision tree and (t) is supervised data. In the online prediction process, the output of the random forest is the integrated output from each decision tree, as shown in Fig.4. The output of each tree is a probability prediction score for each region, including the special label -1. The output i predict region (t) from the random forest represents the region into which the worker's hand will reach with high probability from time t to t + T .
Representing the reaching motion with the feature vector F(t) allows us to store the current and previous positions of the worker's poses in a single vector without imposing any FIGURE 4. Process of predicting the region toward which the worker's hand will reach from current time t to t + T by using a random forest. First, a feature vector F (t ) of the worker's reaching motion is computed from the point cloud data P(t ). The feature F (t ) is then inputted to the random forest, which is obtained by the semi-automatic training process described in Sec. IV. A random forest is a set of binary decision trees, and each tree outputs the probability of the worker's hand reaching a certain region. The outputs of each tree are integrated, and the region with the highest probability is the prediction output from the random forest. kinematic model for the worker's body. Even though F(t) is a high-dimensional binary vector, the fast computation of the reaching motion is achievable. The random forest classifier can ignore unimportant elements inside F(t) (e.g., voxels that have never been occupied by the worker). Each decision tree is trained to identify and disregard unimportant feature values inside the first binary decision tree layers, thereby quickly reducing the feature space for the prediction process. Figure 5 shows a photo of the experimental system used for validation.

V. VALIDATION OF THE PREDICTION METHOD
The validation experiment involves a cooperative assembly task as follows.
1) A robot sequentially assembles six types of parts in the robot's working area. 2) The robot places an assembled part in the corresponding parts boxes (six boxes are located on a table).

3)
Step-by-step assembly instructions for a product are shown to a worker.  4) The worker selects and picks up parts that are necessary for the product from the parts boxes, and assembles the product on a conveyor belt.
In practice, the robot does not assemble parts in the robot's working area, and six types of parts are prepared in the common working area because this situation is sufficient to evaluate the performance of the proposed method and verify the extraction of the worker's cluster from the 3D point cloud, including the robot's cluster.
In the verification experiment, the common working area is coincident with the area where the parts boxes are located. This common working area is divided into six regions, each of VOLUME 8, 2020  Figs.7a and 7b indicates that the worker's hand will not reach the common working space from time t to t + T . The prediction result in Fig.7c illustrates that the proposed method outputs the correct prediction at 0.4 [s] after the initial pose of the worker. The results show that the proposed method generates correct predictions based on slight changes in the worker's posture.
which is represented by a box. In this situation, the proposed method predicts which box the worker's hand will reach.
The system configuration is shown in Fig.6. A Kinect v2 for Windows [36] is used as the 3D sensor and set at 1.2 [m] above the parts boxes. The Kinect v2 captures 512 × 424 depth images at a frequency of 30 Hz. The 3D point cloud data obtained by Kinect v2 are sent to a PC with an Intel Core i7-4720HQ CPU and 16 GB of DDR3 SDRAM, and processed for prediction. A point cloud library [41] is used for processing the 3D point cloud data.
To create the training dataset, the worker performs 300 cycles (50 cycles for each box). Here, starting from the  initial pose of the worker, we define one cycle process to include the worker picking up one part and inserting that part into a fixture located on a conveyor belt. The random forest is constructed using the created training datasets. A method for this machine-learning part is implemented with a machine-learning library called scikit-learn [42]. Table 1 shows the parameter settings for the proposed prediction method. In this experiment, Kinect v2 can measure a 3D space with a volume of 2.
Here, we set an ROI of 1.0[m] × 1.0[m] × 1.0[m] in the observed 3D space. In the ROI, we divide the 3D space by voxels with length = 0.1[m] in each dimension. Thus, the dimension of the binary vector f (t) is equal to 1,000. In addition, the dimension of the feature vector is 4,000 since k 0 is set to be two, as shown in Table 1.
To validate the proposed prediction method, we performed an experiment for 90 cycles (i.e., 15 cycles for each of the six boxes). The number of 3D points obtained by Kinect v2 is almost 217,000. Downsampling is performed at each time point, and the number of obtained 3D points is decreased to nearly 14,000 points. Figures 7 and 8 shows the obtained 3D point cloud, prediction results, and photos for the 65th cycle. In Figs. 7a-8h, the upper, middle, and lower figures show the 3D point cloud with prediction results, photos captured by the camera located above the conveyor belt, and photos captured by the camera located in front of the conveyer belt, respectively. In Fig. 7 and 8, the processes of stretching out, picking up the part, and putting it on the conveyer belt are shown in 7a-7d, 7i-7l, and 8a-8e, respectively. After picking up the part, the method predicted ''no box'' (i.e., the worker will not pick up any part in the current phase), as shown in Figs. 8a-8h. This result and VOLUME 8, 2020 FIGURE 9. Time-series prediction results of the random forest for the cycle shown in Fig.7. The horizontal axis shows time, with t = 0 being the beginning of the reaching motion. The vertical axis shows the output of the random forest. The worker can be considered to be in one out of seven states [i.e., reaching box i ∈ {1, · · · , 6} or not reaching any box (''no box'')]. For this cycle, the proposed method can predict the reaching motion correctly at time 0.4[s] since the output of the random forest changes from ''no box'' to ''Box 1'' at that time. Fig.7c show that the proposed method can produce the correct prediction based on slight changes in the worker's posture. Figure 9 illustrates the result of time-series prediction for the 65th cycle. In this experiment, a total of seven states of the worker can be considered, that is, reaching boxes i, i ∈ {1, 2, · · · , 6} and not reaching any box (''no box''). Thus, Fig.9 Figure 10 shows the statistical prediction result for the entire validation set of 90 reaching motions. Compared to Fig.9, which illustrates the prediction result over time, this graph shows the prediction result over normalized distance ratio, where 0 and 1 are the initial point of the trajectory of the reaching motion and the point where the worker's hand reached the box, respectively. Figure 10 shows that when the worker has performed 40% to 50% of the reaching motion from the worker's initial position, the proposed method can correctly predict the reaching motion. The placement of the boxes with regard to the initial pose of the worker indicated that the reaching motions for the right arm were longest for box 1, with motion's length diminishing for every box down to box 6. Longer arm reaching motion results in more visual features of worker's posture that can be used to distinguish the reaching motion toward different boxes. This would explain the differences in the early prediction results seen in Fig.10 for different targeted boxes. Figure 11 shows the computation time for the prediction process. Each prediction process, including the processing of the 3D point cloud, takes 5. 18 [ms]. This result indicates that real-time prediction can be performed by the proposed method.
The changes in prediction accuracy and computation time with the number of decision trees constructing the random forest are shown in Fig.12. Figure 12 illustrates ''Prediction using Random Forest'' corresponding to Fig.11 since the The horizontal axis shows the distance ratio of trajectories beginning from the initial position to the worker's hand reaching the boxes. The average prediction result for each reaching motion is illustrated in this graph. Based on the 40% to 50% initial trajectory of the reaching motion, the proposed method can correctly predict the reaching motion.  computation time for processing the 3D point cloud and feature vector do not depend on the number of decision trees. This figure shows that the prediction accuracy exceeds more than 85% when the computation time is more than 0.2 [ms], and the accuracy gradually increases up to 90%. This result indicates that the proposed method can perform high-accuracy prediction in real-time.

VI. CONCLUSION
This paper proposed a prediction method for reaching motions used to achieve efficient collaborative tasks by a worker and an industrial robot (F-PREMO). The proposed method has the following three advantages: First, the proposed method does not require the posture of each joint of the worker to be estimated or a skeleton model of the worker. In other words, the method does not impose any restrictions such as attaching markers to the worker or placing a 3D sensor in front of the worker. The second advantage relates to the computation time and prediction accuracy of the proposed method. The experimental results show that the performance of the proposed method is similar to or better than existing methods, without imposing any restrictions for the placement of the 3D sensor. Finally, the proposed method does not require much time and cost to generate training data. In general, the training data utilized for supervised learning methods are created manually, requiring considerable time and cost. To solve this problem, the training data are generated semi-automatically in the proposed method using a random forest.
In future work, we will confirm that the proposed prediction method can be applied to worker motions other than the reaching motion. It is also necessary to consider the method that generates the trajectory of the manipulator by using the prediction obtained by the proposed method.