Versatile kinematics-based constraint identification applied to robot task reproduction

Abstract

Identifying kinematic constraints between a robot and its environment can improve autonomous task execution, for example, in Learning from Demonstration. Constraint identification methods in the literature often require specific prior constraint models, geometry or noise estimates, or force measurements. Because such specific prior information or measurements are not always available, we propose a versatile kinematics-only method. We identify constraints using constraint reference frames, which are attached to a robot or ground body and may have zero-velocity constraints along their axes. Given measured kinematics, constraint frames are identified by minimizing a norm on the Cartesian components of the velocities expressed in that frame. Thereby, a minimal representation of the velocities is found, which represent the zero-velocity constraints we aim to find. In simulation experiments, we identified the geometry (position and orientation) of twelve different constraints including articulated contacts, polyhedral contacts, and contour following contacts. Accuracy was found to decrease linearly with sensor noise. In robot experiments, we identified constraint frames in various tasks and used them for task reproduction. Reproduction performance was similar when using our constraint identification method compared to methods from the literature. Our method can be applied to a large variety of robots in environments without prior constraint information, such as in everyday robot settings.

1 Introduction

Autonomous robotic manipulation has the potential to improve human lives by alleviating physical effort. Robots may offer advantages over human labor regarding consistency, endurance, strength, accuracy, and/or precision in fields such as healthcare, logistics, exploration, and the manufacturing industry. However, autonomous robots are typically designed for one specific task in one specific environment, hence they lack versatility. Tasks in different environments therefore often require different robots, which may be expensive and impractical.

It may therefore be beneficial to develop robots that are versatile in their task execution, allowing a single robot to deal with a variety of tasks and environments. Manipulation tasks may include pick-and-place tasks (e.g., order picking), contact tasks (e.g., wiping, polishing, contour following, and opening/closing compartments) and tool-use tasks (e.g., hammering and screwing). Common environments may include moveable objects such as tools, as well as (immovable) physical constraints.

Models of the physical environments may assist in versatile task execution. For example, door opening is a common everyday task involving similar articulation mechanisms across most doors. If the interaction mechanisms (e.g., hinges and slides) can be modeled, a robot may apply the same control to all environments that have the same mechanisms, thereby improving robot versatility. However, manually creating such models may be cumbersome due to many possible variations in the environment, such as object positions, orientations, shapes, and dynamics. There is therefore a need for automatic modeling of physical environments (Kroemer et al., 2020).

Automatically modeling physical environments may occur in a Learning from Demonstration (LfD) context (Kroemer et al., 2020). In LfD, robots learn to perform tasks from human demonstrations, for example, by manually guiding a robot, rather than by explicit programming. From the demonstration data, the physical interactions throughout the demonstration may be modelled, which in turn may be used in autonomous task reproduction.

Physical environments typically contain kinematic constraints that restrict movement. In everyday settings numerous constraints may be encountered, including articulated/mechanism contacts, such as prismatic (drawers) and revolute joints (doors), polyhedral contacts such as pin-plane contacts (pen drawing) and plane-plane contact (box sliding), and contour-following contacts (dusting).

Constraint awareness can benefit task execution in several ways. First, tasks can often be simplified when expressed in the constraints (Bruyninckx and De Schutter, 1996). Second, choosing a suitable control method, e.g., position, velocity, force, or impedance control can improve both task performance and stability by keeping undesirable interaction forces low (Conkey and Hermans, 2019; De Schutter et al., 1999). Third, planning to avoid constraints can simplify some tasks because there are fewer state transitions to consider, such as in reaching tasks (Oriolo and Vendittelli, 2009). Alternatively, planning to introduce constraints can simplify some tasks by reducing the free space of the robot, for example, during object alignment (Suomalainen et al., 2021). Fourth, differentiating between constrained states in a task provides meaningful, tractable building blocks, which can improve task planning (Ureche et al., 2015; Jain and Niekum, 2018; Holladay et al., 2021) and facilitate learning (Simonič et al., 2024). Fifth, some constraints are common in many robot tasks and can therefore be a basis for generalization between tasks (Li and Brock, 2022; Li et al., 2023).

Kinematic constraints consist of a (i) constraint class, e.g., a type of joint or contact, and (ii) constraint geometry, e.g., the orientation and position of a rotation axis or surface normal (De Schutter et al., 1999). Identifying constraints from data therefore requires (i) classifying the constraint class and (ii) identifying the constraint geometry, both are the topic of this work.

In this work, we make three assumptions about robot manipulation to limit our scope. First, we assume that robots are rigidly attached to a constrained mechanism or object in contact with the environment, and thus leave grasping and environmental compliance out of our scope. Second, we assume that there is a single maintained contact between a robot and the environment, such as a contact point, line, plane, or other continuous contact area. Third, we assume that all unconstrained degrees of freedom are excited, to prevent ambiguity between true physical constraints that do not allow motion and non-observed motions.

Bruyninckx and De Schutter defined kinematic constraint classes based on contact (Bruyninckx and De Schutter, 1993a; Bruyninckx et al., 1993b). They proposed to identify constraints by two methods: first, based on the absence of mechanical power in the direction of constraints, requiring position and force measurements. Second, based on whether velocities or forces separately fit candidate constraint models. They used both methods in a Kalman filter with a candidate constraint model of a specific class to estimate constraint geometry, such as contact points, axes of rotations, and polyhedral contact normals (De Schutter et al., 1999).

Several authors extended the methods of Bruyninckx and De Schutter, mainly on multi-contact polyhedral contacts (Meeussen et al., 2007; Cabras et al., 2010; Lefebvre et al., 2005). They identify, classify, and segment data containing arbitrary contacts between two uncertain polyhedral or curved objects using pose and wrench measurements. Where previous methods required approximate geometric models of the polyhedra, Slaets et al. identify arbitrary unknown polyhedra at runtime (Slaets et al., 2007). Although polyhedrons are useful to approximate many tasks, these methods are fundamentally limited in modeling rotations.

Alternative methods omit noise models, and fit constraint models of a specific class directly to data, identifying the constraint geometry in the process. Such models are specified manually, and when several candidate models are proposed, the best-fitting model is chosen. For example, several authors identify one of three candidate models (fixed, prismatic, and revolute joints) using kinematic measurements (Sturm et al., 2010; Niekum et al., 2015; Hausman et al., 2015). Subramani et al. identify six candidate models using kinematic measurements (Subramani et al., 2018). They later expand their method with force information and identify eight candidate models (Subramani et al., 2020).

Some methods do not require specific constraint models but use more versatile representations to capture multiple constraints. Sturm et al. fit Gaussian processes in configuration space, but the identified Gaussian process kernel parameters are not straightforward to interpret (Sturm et al., 2011). Mousavi Mohammadi et al. identify “task frames” instead of identifying constraints explicitly (Mousavi Mohammadi et al., 2024). Such frames conveniently describe contact tasks and thereby often align with constraint geometry, but lack constraint classification (Bruyninckx and De Schutter, 1996). They use velocities and/or forces in a multi-step decision process to identify frame properties that result in low or constant velocities and forces with minimal uncertainty. Van der Walt et al. identify constraints from kinematic data by fitting points, lines, planes, and their higher dimensional equivalents in six dimensional linear and angular velocity space, after which they select the best fitting model (Van der Walt et al., 2025).

Because kinematic constraints occur often in robot manipulation tasks, we believe more versatile constraint identification is an important step to advance the applicability of robots in the real world. However, the literature on constraint identification shows two common limitations. First, methods in the literature often require specific prior constraint models and estimates of geometry and noise. In everyday robot settings where many constraints can be encountered, it may be challenging to exhaustively define such models and estimates. Second, various methods require force measurements, which may not be available. This work overcomes both limitations by identifying constraint frames from kinematic data. First, the method can identify a wide variety of constraints without specific prior constraint models, or estimates of geometry or noise. The method is used to identify articulated/mechanism contacts, polyhedral contacts, and contour following contacts. Second, the method only requires kinematic measurements. Therefore, our method can be applied to various robots in everyday settings, without prior information about the environment or task.

The method offers two more useful features. First, the identified constraint frames can be used directly for task reproduction since the constraints are expressed in a task-relevant reference frame. Second, the number of identified parameters is fixed, in contrast to methods that use an increasing number of parameters for each added candidate model.

This paper is organized as follows: Section 2 discusses preliminaries on rigid body kinematics. Section 3 proposes to specify constraints using constraint frames. Section 4 proposes an optimization problem to identify constraint frames from kinematic data. Section 5 evaluates the method on experimental data from simulation and real robot task demonstrations. Section 6 reproduces robot tasks using the identified constraint frames. Section 7 discusses the results and draws conclusions.

2 Preliminaries on kinematics

This section contains preliminaries for rigid bodies kinematics (Lynch and Park, 2017). Motion between two rigid bodies can be represented by the transformation between two reference frames, one frame rigidly attached to each body. Such reference frames have a position and orientation in space, which can change with body motion. The transformation between two reference frames in three-dimensional space can be parameterized by a distance between their positions $\mathit{p}\in {\mathbb{R}}^3$ and a rotation matrix $\mathit{R}\in SO\left(3\right)$ between their orientations. These components can be combined in a homogenous transformation matrix

$$\mathit{T}=\left[\begin{array}{cc}\mathit{R}& \mathit{p}\\ {\mathbf{0}}_{1\times 3}& 1\end{array}\right]\in SE\left(3\right),$$

which represents the pose (orientation $\mathit{R}$ and position $\mathit{p}$ ) of one frame relative to the other.

The velocity between two rigid bodies can be represented by a twist $\mathcal{V}={\left[{\mathit{\omega }}^T,{\mathit{v}}^T\right]}^T\in {\mathbb{R}}^6$ , which contains an angular velocity $\mathit{\omega }\in {\mathbb{R}}^3$ and a linear velocity $\mathit{v}\in {\mathbb{R}}^3$ . The twist must be expressed in a reference frame, denoted by a superscript, e.g., ${\mathcal{V}}^b={\left[{\left({\mathit{\omega }}^b\right)}^T,{\left({\mathit{v}}^b\right)}^T\right]}^T$ for reference frame $\left\{b\right\}$ . If $\mathit{T}$ describes the pose of a body frame $\left\{b\right\}$ with respect to a ground frame $\left\{g\right\}$ , the twist ${\mathcal{V}}^b$ of a body with respect to the ground expressed in frame $\left\{b\right\}$ is related to $\mathit{T}$ by:

$${\mathit{T}}^{-1}\dot{\mathit{T}}={\left[{\mathcal{V}}^b\right]}_{\times }=\left[\begin{array}{cc}{\left[{\mathit{\omega }}^b\right]}_{\times }& {\mathit{v}}^b\\ {\mathbf{0}}_{1\times 3}& 0\end{array}\right]\in se\left(3\right),$$ (1)

where ${\left[\mathit{\omega }\right]}_{\times }$ is the skew-symmetric representation of $\mathit{\omega }={\left[{\omega }_1,{\omega }_2,{\omega }_3\right]}^T$ :

$${\left[\mathit{\omega }\right]}_{\times }=\left[\begin{array}{ccc}0& -{\omega }_3& {\omega }_2\\ {\omega }_3& 0& -{\omega }_1\\ -{\omega }_2& {\omega }_1& 0\end{array}\right]\in so\left(3\right).$$

Expressing the twist ${\mathcal{V}}^b$ in a frame other than $\left\{b\right\}$ requires the pose matrix of $\left\{b\right\}$ with respect to the new frame. For example, if $\mathit{R}$ and $\mathit{p}$ again represent the pose of body frame $\left\{b\right\}$ with respect to the ground frame $\left\{g\right\}$ , the transformation

$${\mathcal{V}}^g=\left[\begin{array}{cc}\mathit{R}& {\mathbf{0}}_{3\times 3}\\ {\left[\mathit{p}\right]}_{\times }\mathit{R}& \mathit{R}\end{array}\right]{\mathcal{V}}^b.$$ (2)

expresses the twist ${\mathcal{V}}^b$ in frame $\left\{g\right\}$ .

3 Constraint frames

This section introduces a method to specify constraints that result in constrained kinematics. This method is used in Section 4 for the inverse problem: identifying constraints from kinematic data.

3.1 Degrees of freedom

Kinematic constraints limit a robot’s motion. We consider Pfaffian constraints on the end effector twist $\mathcal{V}$ of a robot with respect to the ground. The form of such constraints is

$$\mathit{A}\left(\mathit{T}\right)\mathcal{V}={\mathbf{0}}_h,$$ (3)

where $\mathit{A}\left(\mathit{T}\right)\in {\mathbb{R}}^{h\times 6}$ defines $h\le 6$ constraint equations (Lynch and Park, 2017).

We restrict $\mathit{A}\left(\mathit{T}\right)$ to be coordinate transformations of the form of Equation 2 that transform velocities $\mathit{\omega }$ and/or $\mathit{v}$ to one or more axes of some to be determined frame $\left\{\xi \right\}$ . For example, the $i$ -th constraint equation ${\mathit{A}}_i\left(\mathit{T}\right)$ may restrict the $j$ -th axis of $\mathit{\omega }$ when expressed in $\left\{\xi \right\}$ , such that ${\mathit{A}}_i\left(\mathit{T}\right)\mathcal{V}={\omega }_j^{\xi }=0.$ Constraints can similarly be applied to $\mathit{v}$ .

We also allow constraints on $\mathit{\omega }$ and $\mathit{v}$ to be described from different frames, respectively. We denote the frame for $\mathit{\omega }$ as $\left\{\xi \right\}=\left\{\varphi \right\}$ and for $\mathit{v}$ as $\left\{\xi \right\}=\left\{\psi \right\}$ . Therefore, we consider constraints of the form:

$$\begin{array}{c}{\omega }_n^{\varphi }=0,\\ v_m^{\psi }=0.\end{array}$$ (4)

Here, axis $n$ of $\mathit{\omega }$ is constrained in $\left\{\varphi \right\}$ , and axis $m$ of $\mathit{v}$ is constrained in $\left\{\psi \right\}$ . For either velocity, the number of constraints can be none, one, two, or three. We thus consider bilateral zero-velocity constraints along one or more axes of $\mathit{\omega }$ expressed in $\left\{\varphi \right\}$ and $\mathit{v}$ expressed in $\left\{\psi \right\}$ . Although most contact constraints are unilateral, they can be modeled as bilateral if contact is kept, which we assume in this work.

To specify whether the axes of ${\mathit{\omega }}^{\varphi }$ are constrained according to (4), we use an associated binary degree of freedom vector ${\mathit{d}}^{\varphi }\in {\left\{0,1\right\}}^3$ . Here, $0$ indicates the associated axis is constrained and $1$ indicates it is unconstrained. The analogous situation applies to $\mathit{v}$ with ${\mathit{d}}^{\psi }\in {\left\{0,1\right\}}^3$ .

By summing the degree of freedom vector ${\sum }_{n=1}^3{d}_n^{\varphi }\in \left\{0,1,2,3\right\}$ , we obtain the physical degrees of freedom of $\mathit{\omega }$ in frame $\left\{\varphi \right\}$ . When adding units, ${\mathit{d}}^{\varphi }\text{\hspace{0.17em}rad\hspace{0.17em}}{\mathrm{s}}^{-1}=\text{span}\left({\mathit{\omega }}^{\varphi }\right)$ gives a basis for $\mathit{\omega }$ , which represents all possible $\mathit{\omega }$ being either zero, lying on a line, on a plane, or in a volume.

Constraints can be thus defined by specifying ${\mathit{d}}^{\varphi }$ and ${\mathit{d}}^{\psi }$ . For example, a revolute joint can be specified by ${\mathit{d}}^{\varphi }={\left[1,0,0\right]}^T$ if the first axis of $\left\{\varphi \right\}$ aligns with the rotation axis, and ${\mathit{d}}^{\psi }={\left[0,0,0\right]}^T$ if the position of $\left\{\psi \right\}$ is on the rotation axis.

3.2 Frame type and geometry

So far, the frame $\left\{\varphi \right\}$ in which to express constraints on $\mathit{\omega }$ , and frame $\left\{\psi \right\}$ in which to express constraints on $\mathit{v}$ , have not been specified. Here, we require the frames to be attached to the ground body $\mathcal{G}$ or the robot body $\mathcal{B}$ , similar to Mousavi Mohammadi et al. (2024).

The $\mathit{v}$ frame $\left\{\psi \right\}$ must have its orientation constant in $\mathcal{G}$ or $\mathcal{B}$ , and its position constant in $\mathcal{G}$ or $\mathcal{B}$ . This results in four frame types denoted by $\left\{\psi \right\}\in \left\{\left\{\mathcal{G}\mathcal{G}\right\},\left\{\mathcal{G}\mathcal{B}\right\},\left\{\mathcal{B}\mathcal{G}\right\},\left\{\mathcal{B}\mathcal{B}\right\}\right\}$ . For example, $\left\{\mathcal{B}\mathcal{G}\right\}$ indicates that the frame’s orientation is constant in ground body $\mathcal{B}$ and its position is constant in body $\mathcal{G}$ . Figure 1 visualizes these frame types during motion.

FIGURE 1.

Constraint frame identification following a two-dimensional task demonstration, e.g., by a human guiding the robot. The behavior of the constraint frame types $\left\{\psi \right\}\in \left\{\left\{\mathcal{G}\mathcal{G}\right\},\left\{\mathcal{G}\mathcal{B}\right\},\left\{\mathcal{B}\mathcal{G}\right\},\left\{\mathcal{B}\mathcal{B}\right\}\right\}$ is shown at two instants (solid and dashed) during a demonstration of robot body $\mathcal{B}$ with respect to ground body $\mathcal{G}$ . Frames $\left\{\mathcal{G}\mathcal{G}\right\}$ and $\left\{\mathcal{B}\mathcal{B}\right\}$ are rigidly attached to their respective bodies. Frames $\left\{\mathcal{G}\mathcal{B}\right\}$ and $\left\{\mathcal{B}\mathcal{G}\right\}$ have constant orientation in one body, but constant position in the other. Only the transformations between ground $\left\{g\right\}$ and body $\left\{b\right\}$ are measured (black), and the method identifies the unknown (gray) constraint between the tool and ground. In this example, the optimal frame is $\left\{\mathcal{G}\mathcal{B}\right\}$ at tip of the tool, because the linear velocities along one of its axes are always zero, which is not the case for velocities expressed in the other frames. In practice, the method performs a continuous optimization in three-dimensional space to find the frame position and orientation, and also identifies the angular velocity constraint frame.

Because the frames’ orientations and positions are constant in either one of the bodies, they can be parameterized by a constant rotation matrix ${\mathit{R}}^{\psi }$ and position vector ${\mathit{p}}^{\psi }$ . For example, ${\mathit{R}}^{\mathcal{B}\mathcal{G}}$ denotes the constant rotation matrix between $\left\{\mathcal{B}\mathcal{G}\right\}$ and a frame $\left\{b\right\}$ on body $\mathcal{B}$ (Figure 1).

For the $\mathit{\omega }$ frame $\left\{\varphi \right\}$ , only two types of constraint frames have to be considered because $\mathit{\omega }$ is independent of the position of the frame it is expressed in, that is ${\mathit{\omega }}^{\mathcal{G}\mathcal{G}}={\mathit{\omega }}^{\mathcal{G}\mathcal{B}}$ and ${\mathit{\omega }}^{\mathcal{B}\mathcal{G}}={\mathit{\omega }}^{\mathcal{B}\mathcal{B}}$ . Therefore, the dependence on frame position can be omitted. The two frame types are $\left\{\varphi \right\}\in \left\{\left\{\mathcal{G}\right\},\left\{\mathcal{B}\right\}\right\}$ , which are parameterized solely by a constant rotation matrix ${\mathit{R}}^{\varphi }$ . Because frames $\left\{\varphi \right\}$ do not have a position, they are shown at convenient positions in figures.

3.3 Applied constraint frames

In summary, degrees of freedom ${\mathit{d}}^{\varphi }$ and ${\mathit{d}}^{\psi }$ specify whether axes are constrained; types $\left\{\varphi \right\}\in \left\{\left\{\mathcal{G}\right\},\left\{\mathcal{B}\right\}\right\}$ and $\left\{\psi \right\}\in \left\{\left\{\mathcal{G}\mathcal{G}\right\},\left\{\mathcal{G}\mathcal{B}\right\},\left\{\mathcal{B}\mathcal{G}\right\},\left\{\mathcal{B}\mathcal{B}\right\}\right\}$ specify what the frames are attached to; and ${\mathit{R}}^{\mathit{\varphi }}$ , ${\mathit{R}}^{\mathit{\psi }}$ , and ${\mathit{p}}^{\mathit{\psi }}$ specify the geometry of the constraints. The combination of degrees of freedom and frame types can be considered the class of a constraint. Table 1 summarizes these properties and the constant parameters that specify them. Figure 2 and Table 2 show twelve (not exhaustive) example constraints that can be specified using this method.

TABLE 1.

Parameters of the proposed method.

Velocity ${\mathbb{R}}^3$	Constraint frame
	DOF	Type	Geometry
	Binary ${\left\{0,1\right\}}^3$	$\left\{const.pos.vectorin,\right.$ $\left.const.rot.matrixin\right\}$	Rot. matrix $SO\left(3\right)$	Pos. vector ${\mathbb{R}}^3$
$\mathit{\omega }$ (angular)	${\mathit{d}}^{\varphi }$	$\left\{\varphi \right\}\in \left\{\left\{\mathcal{G}\right\},\left\{\mathcal{B}\right\}\right\}$	${\mathit{R}}^{\varphi }$	$-$
$\mathit{v}$ (linear)	${\mathit{d}}^{\psi }$	$\left\{\psi \right\}\in \left\{\left\{\mathcal{B}\mathcal{B}\right\},\left\{\mathcal{G}\mathcal{B}\right\},\left\{\mathcal{G}\mathcal{G}\right\},\left\{\mathcal{B}\mathcal{G}\right\}\right\}$	${\mathit{R}}^{\mathit{\psi }}$	${\mathit{p}}^{\mathit{\psi }}$

FIGURE 2.

Visualization of twelve (not exhaustive) constraints that can be captured by the method. Each constraint (A–L) is described in Table 2. The left-hand figures are planar, where the rotation axis (y-axis) is normal to the plane. Colors (Table 2) indicate the linear velocity constraint frame type $\left\{\psi \right\},\hspace{0.5em}\mathrm{where}\hspace{0.5em}\left\{\mathcal{G}\mathcal{G}\right\}\hspace{0.5em}\mathrm{is}\hspace{0.5em}\mathrm{yellow},\left\{\mathcal{G}\mathcal{B}\right\}\hspace{0.5em}\mathrm{is}\hspace{0.5em}\mathrm{green},\left\{\mathcal{B}\mathcal{G}\right\}\hspace{0.5em}\mathrm{is}\hspace{0.5em}\mathrm{red},\mathrm{and}\left\{\mathcal{B}\mathcal{B}\right\}\hspace{0.5em}\mathrm{is}\hspace{0.5em}\mathrm{blue}$ . For conciseness, angular velocity constraint frames $\left\{\varphi \right\}$ overlap exactly with the linear velocity constraint frames $\left\{\psi \right\}$ , such that ${\mathit{R}}^{\mathit{\varphi }}={\mathit{R}}^{\mathit{\psi }}$ . If a frame at the second time instance (dashed) is not shown, it is the same as the first. The linear velocity degrees of freedom ${\mathit{d}}^{\mathit{\psi }}$ are marked along the frame axes by an arrowhead (unconstrained) or no arrowhead (constrained). The unconstrained angular velocity degrees of freedom ${\mathit{d}}^{\mathit{\varphi }}$ are marked by a rotational vector.

TABLE 2.

Parameters of the example constraints of Figure 2.

Figure 2 label	Constraint name	Constraint frame
		DOF	Type	Geometry
		${\mathit{d}}^{\varphi }$	$\left\{\varphi \right\}$	${\mathit{R}}^{\varphi }$	$-$
		${\mathit{d}}^{\psi }$	$\left\{\psi \right\}$	${\mathit{R}}^{\mathit{\psi }}$	${\mathit{p}}^{\psi }$
A	Revolute joint	$\left[0,1,0\right]$	$-$	$\left[-,{\mathit{r}}_y,-\right]$	$-$
		$\left[0,0,0\right]$	$-$	$\left[-,-,-\right]$	$\left[p_x,-,p_z\right]$
B	Prismatic joint	$\left[0,0,0\right]$	$-$	$\left[-,-,-\right]$	$-$
		$\left[1,0,0\right]$	$-$	$\left[{\mathit{r}}_x,-,-\right]$	$\left[-,-,-\right]$
C	Cylinder joint	$\left[1,0,0\right]$	$-$	$\left[{\mathit{r}}_x,-,-\right]$	$-$
		$\left[1,0,0\right]$	$-$	$\left[{\mathit{r}}_x,-,-\right]$	$\left[-,p_y,p_z\right]$
D	Plane-plane contact	$\left[0,0,1\right]$	$-$	$\left[-,-,{\mathit{r}}_z\right]$	$-$
		$\left[1,1,0\right]$	$-$	$\left[-,-,{\mathit{r}}_z\right]$	$\left[-,-,-\right]$
E	Planar contour following	$\left[0,1,0\right]$	$-$	$\left[-,{\mathit{r}}_y,-\right]$	$-$
		$\left[1,0,0\right]$	$\left\{\mathcal{B}\mathcal{B}\right\}$	$\left[{\mathit{r}}_x,-,-\right]$	$\left[p_x,-,-\right]$
F	Planar contour rolling	$\left[0,1,0\right]$	$-$	$\left[-,{\mathit{r}}_y,-\right]$	$-$
		$\left[1,0,0\right]$	$\left\{\mathcal{G}\mathcal{G}\right\}$	$\left[{\mathit{r}}_x,-,-\right]$	$\left[p_x,-,-\right]$
G	Cart on a curved surface	$\left[1,1,1\right]$	$-$	$\left[-,-,-\right]$	$-$
		$\left[1,1,0\right]$	$\left\{\mathcal{B}\mathcal{B}\right\}$	$\left[-,-,{\mathit{r}}_z\right]$	$\left[p_x,p_y,p_z\right]$
H	Contour rolling	$\left[1,1,1\right]$	$-$	$\left[-,-,-\right]$	$-$
		$\left[1,0,0\right]$	$\left\{\mathcal{G}\mathcal{G}\right\}$	$\left[{\mathit{r}}_x,-,-\right]$	$\left[p_x,p_y,p_z\right]$
I	Planar pin-plane contact	$\left[0,1,0\right]$	$-$	$\left[-,{\mathit{r}}_y,-\right]$	$-$
		$\left[1,0,0\right]$	$\left\{\mathcal{G}\mathcal{B}\right\}$	$\left[{\mathit{r}}_x,-,-\right]$	$\left[p_x,-,p_z\right]$
J	Planar plane-pin contact	$\left[0,1,0\right]$	$-$	$\left[-,{\mathit{r}}_y,-\right]$	$-$
		$\left[1,0,0\right]$	$\left\{\mathcal{B}\mathcal{G}\right\}$	$\left[{\mathit{r}}_x,-,-\right]$	$\left[p_x,-,p_z\right]$
K	Pin-plane contact	$\left[1,1,1\right]$	$-$	$\left[-,-,-\right]$	$-$
		$\left[1,1,0\right]$	$\left\{\mathcal{G}\mathcal{B}\right\}$	$\left[-,-,{\mathit{r}}_z\right]$	$\left[p_x,p_y,p_z\right]$
L	Plane-pin contact	$\left[1,1,1\right]$	$-$	$\left[-,-,-\right]$	$-$
		$\left[1,1,0\right]$	$\left\{\mathcal{B}\mathcal{G}\right\}$	$\left[-,-,{\mathit{r}}_z\right]$	$\left[p_x,p_y,p_z\right]$

“ $-$ ” means that entry is not uniquely defined.

The presented method does not uniquely define constraints for three reasons. First, the ordering of the axes is undefined. For example, $\mathit{d}={\left[1,0,0\right]}^T$ and $\mathit{R}=\left[{\mathit{r}}_1,{\mathit{r}}_2,{\mathit{r}}_3\right]$ define the same constraint as $\mathit{d}={\left[0,1,0\right]}^T$ and $\mathit{R}=\left[{\mathit{r}}_2,{\mathit{r}}_1,{\mathit{r}}_3\right]$ . Second, the positive or negative directions of the axes are irrelevant because we consider bilateral constraints. Therefore, the basis vectors in $\mathit{R}=\left[\pm {\mathit{r}}_1,\pm {\mathit{r}}_2,\pm {\mathit{r}}_3\right]$ can have either sign. Third, (parts of) $\mathit{R}$ are always free to choose, as shown in Table 2.

In addition, two types of redundancies can occur depending on the constraint. First, (parts of) the frame $\left\{\psi \right\}$ positions ${\mathit{p}}^{\psi }$ may be free to choose. For example, the frame $\left\{\psi \right\}$ of a cylinder joint (Figure 2C) may lie anywhere on its rotation axis. Second, multiple frame types may lead to the same constraints. For example, any of the four frame types of $\left\{\psi \right\}$ can be used to specify a revolute joint (Figure 2A).

For simplicity, we consider angular velocity constraints where either type $\left\{\varphi \right\}\in \left\{\left\{\mathcal{G}\right\},\left\{\mathcal{B}\right\}\right\}$ leads to the same constraint, i.e., when ${\sum }_{n=1}^3{d}_n^{\varphi }\ne 2$ .

4 Constraint frame identification

Section 3 introduced a method to specify constraints that result in constrained kinematics. This section considers the inverse problem: identifying constraints from constrained kinematic data. Constraint identification is first defined as an optimization problem for angular velocity $\mathit{\omega }$ , followed by a second optimization problem for linear velocity $\mathit{v}$ .

Section 3 specified constraints on $\mathit{\omega }$ through a constraint frame $\left\{\varphi \right\}$ with a degree of freedom vector ${\mathit{d}}^{\varphi }$ . The inverse problem, given measured $\mathit{\omega }$ , is to find some $\left\{\varphi \right\}$ that results in some ${\mathit{d}}^{\varphi }$ . To resolve this ambiguity, we aim to find the $\left\{\varphi \right\}$ in which ${\mathit{\omega }}^{\varphi }$ has the minimum number of degrees of freedom and thus the maximum number of constraints. Thus, we define our constraint frame identification as:

$$\underset{\varphi }{\min }{\displaystyle \sum _{n=1}^3}{d}_n^{\varphi }\in \left\{0,\dots ,3\right\}.$$ (5)

Thereby, a minimal representation of $\mathit{\omega }$ is found by expressing it as ${\mathit{\omega }}^{\varphi }$ .

Measuring $\mathit{\omega }$ at samples $k\in \left\{1,2,\dots ,K\right\}$ yields a $3\times K$ matrix $\mathbf{\Omega }$ with rows ${\mathit{\omega }}_n$ , columns $\mathit{\omega }\left(k\right)$ , and entries ${\omega }_n\left(k\right)$ . If the constraint condition of Equation 4 holds for an axis $n$ , then ${\omega }_n^{\varphi }\left(k\right)=0$ should hold for all samples $k$ . To assess the velocity magnitude in measurement data we use the scaled $2$ -norm, or root mean square (RMS), over all samples of an axis $n$ :

$${\lambda }_n^{\varphi }={‖{\mathit{\omega }}_n^{\varphi }‖}_2,$$

as defined by the scaled $p$ -norm with $p=2$ :

$${‖\mathit{x}‖}_p={\left(\frac{1}{K}{\displaystyle \sum _{k=1}^K}{\left|x\left(k\right)\right|}^p\right)}^{\frac{1}{p}},$$

for a signal $\mathit{x}$ with samples $x\left(k\right)$ (Bullen, 2003).

With noiseless measurements, the constraint condition (Equation 4) can then be tested by $d_n^{\varphi }=0$ if ${\lambda }_n^{\varphi }=0$ and $d_n^{\varphi }=1$ if ${\lambda }_n^{\varphi }>0$ . When noise is present however, noise is added to the zero-velocity constraint and the constraint condition (Equation 4) becomes

$${\omega }_n^{\varphi }\left(k\right)\sim \mathcal{N}\left(0,{\sigma }^2\right),$$

assuming normally distributed noise $\mathcal{N}$ with variance ${\sigma }^2$ . Therefore, the norm ${\lambda }_n^{\varphi }$ is not bounded from below by $0$ , but by the noise variance ${\sigma }^2$ , which is the RMS of a normally distributed discrete signal with zero mean. Because ${\lambda }_n^{\varphi }\ge {\sigma }^2>0$ , evaluating $d_n^{\varphi }$ will always lead to $d_n^{\varphi }=1$ , and an optimization of Equation 5 has no gradient.

Instead of minimizing over $d_n^{\varphi }$ , we minimize ${\lambda }_n^{\varphi }$ directly as a continuous measure of a signal’s degree of freedom. We aim to find a frame $\left\{\varphi \right\}$ in which the variance of each of the axes ${\lambda }_n^{\varphi }$ in ${\mathit{\lambda }}^{\varphi }\in {\mathbb{R}}^3$ are minimized. The values in ${\mathit{\lambda }}^{\varphi }$ are bounded from below by ${\sigma }^2$ if constraints are present, but are larger otherwise. Because we are interested in finding those constraints, we prefer finding small individual values in ${\mathit{\lambda }}^{\varphi }$ rather than larger values.

Therefore, we use a $p$ -norm over the three axes of ${\mathit{\lambda }}^{\varphi }$ :

$$\underset{\varphi }{\min }{‖{\mathit{\lambda }}^{\varphi }‖}_p,$$

but with $-\infty <p<1$ , for which the minimization is more sensitive to small individual values in ${\mathit{\lambda }}^{\varphi }$ in contrast to $p>1$ . Some values of $p$ yield special simplified expressions (Bullen, 2003). For example, $p=1$ yields the mean of ${\mathit{\lambda }}^{\varphi }$ , which is equally sensitive to all values in ${\mathit{\lambda }}^{\varphi }$ regardless of size. The limit $p=-\infty$ yields the minimum of ${\mathit{\lambda }}^{\varphi }$ , which is only sensitive to the smallest value in ${\mathit{\lambda }}^{\varphi }$ . Here we use $p=0$ , which yields the following simplified expression:

$$\underset{\varphi }{\min }{‖{\mathit{\lambda }}^{\varphi }‖}_{p=0}=\underset{\varphi }{\min }{\left({\displaystyle \prod _{n=1}^3}{\lambda }_n^{\varphi }\right)}^{1/3},$$

also known as the geometric mean (Bullen, 2003). This results in a quasinorm, because not all conditions for a norm are satisfied. By substituting the $2$ -norms (RMS) and $0$ -quasinorm (geometric mean) into one entry-wise $2,0$ -quasinorm, we obtain

$$\underset{\varphi }{\min }{‖{\mathbf{\Omega }}^{\varphi }‖}_{2,0}=\underset{\varphi }{\min }{\left({\displaystyle \prod _{n=1}^3}{\left(\frac{1}{K}{\displaystyle \sum _{k=1}^K}{\left|{\omega }_n^{\varphi }\left(k\right)\right|}^2\right)}^{\frac{1}{2}}\right)}^{\frac{1}{3}}.$$ (6)

Section 3 proposed two options for $\left\{\varphi \right\}\in \left\{\left\{\mathcal{G}\right\},\left\{\mathcal{B}\right\}\right\}$ , both parameterized by constant ${\mathit{R}}^{\varphi }$ , and thus two optimization problems. The measurement matrix $\mathbf{\Omega }$ can be expressed in frame $\left\{\varphi \right\}$ using the coordinate transformation of Equation 2, yielding

$$\underset{{\mathit{R}}^{\varphi }}{\min }{‖{\mathit{R}}^{\varphi }\mathbf{\Omega }‖}_{2,0},$$ (7)

where the first optimization $\left.\left\{\varphi \right.\}=\{\mathcal{B}\right\}$ uses $\mathbf{\Omega }={\mathbf{\Omega }}^b$ and the second optimization $\left\{\varphi \right\}=\left\{\mathcal{G}\right\}$ uses $\mathbf{\Omega }={\mathbf{\Omega }}^g$ .

This method can also be applied to linear velocity $\mathit{v}$ , with a measured $3\times K$ matrix $\mathit{V}$ :

$$\underset{\psi }{\min }{‖{\mathit{V}}^{\psi }‖}_{2,0}.$$

Section 3 proposed four options for $\left\{\psi \right\}\in \left\{\left\{\mathcal{G}\mathcal{G}\right\},\left\{\mathcal{G}\mathcal{B}\right\},\left\{\mathcal{B}\mathcal{G}\right\},\left\{\mathcal{B}\mathcal{B}\right\}\right\}$ , all parameterized by constant ${\mathit{R}}^{\mathit{\psi }}$ and ${\mathit{p}}^{\mathit{\psi }}$ , and thus four optimization problems. The measured $\mathit{V}$ can be expressed in frame $\left\{\psi \right\}$ using the coordinate transformation of Equation 2, yielding

$$\underset{{\mathit{R}}^{\psi },{\mathit{p}}^{\psi }}{\min }{‖{\mathit{R}}^{\psi }\overline{\mathit{R}}\left(\overline{\mathit{V}}+{\left[{\mathit{p}}^{\psi }\right]}_{\times }\overline{\mathbf{\Omega }}\right)‖}_{2,0},$$ (8)

where $\overline{\mathbf{\Omega }}$ , $\overline{\mathit{V}}$ , $\overline{\mathit{R}}$ are given in Table 3, depending on which of the four frame types they are expressed in.

TABLE 3.

Kinematic variables to transform (Equation 8).

$\left\{\psi \right\}$	$\overline{\mathbf{\Omega }}$ from (2)	$\overline{\mathit{V}}$ from (2)	$\overline{\mathit{R}}$
$\left\{\mathcal{B}\mathcal{B}\right\}$	${\overline{\mathbf{\Omega }}}^b$	${\overline{\mathit{V}}}^b$	$-\left[{\mathit{I}}_{3\times 3},{\mathit{I}}_{3\times 3},\dots \right]$
$\left\{\mathcal{G}\mathcal{B}\right\}$	${\overline{\mathbf{\Omega }}}^b$	${\overline{\mathit{V}}}^b$	$\left[\mathit{R}\left(0\right),\mathit{R}\left(1\right),\dots \right]$
$\left\{\mathcal{G}\mathcal{G}\right\}$	${\overline{\mathbf{\Omega }}}^g$	${\overline{\mathit{V}}}^g$	$-\left[{\mathit{I}}_{3\times 3},{\mathit{I}}_{3\times 3},\dots \right]$
$\left\{\mathcal{B}\mathcal{G}\right\}$	${\overline{\mathbf{\Omega }}}^g$	${\overline{\mathit{V}}}^g$	$\left[{\mathit{R}}^T\left(0\right),{\mathit{R}}^T\left(1\right),\dots \right]$

Because the ground-truth constraint frame types $\left\{\varphi \right\}$ and $\left\{\psi \right\}$ will not be known beforehand, both optimizations of Equation 7 and all four of Equation 8 must be done. Then, $\left\{\varphi \right\}$ and $\left\{\psi \right\}$ can be classified by choosing the type with the lowest minimized quasinorm. Furthermore, the binary degrees of freedom vectors $\mathit{d}$ can be classified by thresholding $\mathit{\lambda }$ .

The inputs to the method are twists $\mathcal{V}\left(k\right)$ containing $\mathit{\omega }\left(k\right)$ in $\mathbf{\Omega }$ and $\mathit{v}\left(k\right)$ in $\mathit{V}$ , and poses $\mathit{T}\left(k\right)$ containing $\mathit{R}\left(k\right)$ and $\mathit{p}\left(k\right)$ . The outputs are one set of the constraint frame parameters of Table 1: degrees of freedom, type, and geometry.

5 Evaluation

To evaluate the identification method, we simulated kinematic data for all twelve constraints of Figure 2 and gathered experimental robot data for five constraints. For the simulation experiments we used prior knowledge of the constraint frame types $\left\{\varphi \right\}$ and $\left\{\psi \right\}$ , to test how identification accuracy is affected by noise added to the poses $\mathit{T}\left(k\right)$ , and consequently twists $\mathcal{V}\left(k\right)$ . For the robot experiments we used no prior knowledge of the frame type. We classified the constraint frame types and identified the constraint geometry, with real-life noise.

5.1 Simulation experiments

For the simulation experiments, we first generated constrained end effector poses $\mathit{T}\left(k\right)$ using Python 3.11 and NumPy 1.26. The velocities were ${\mathit{\omega }}^{\varphi }\left(k\right)={\mathit{d}}^{\varphi }\circ 0.5\circ {\left[1+\sin \left(2k\Delta \mathrm{t}\right),-1,\cos \left(3k\Delta \mathrm{t}\right)\right]}^T\text{\hspace{0.17em}rad\hspace{0.17em}}{\mathrm{s}}^{-1}$ and ${\mathit{v}}^{\psi }\left(k\right)={\mathit{d}}^{\psi }\circ 0.1\circ {\left[-1,\cos \left(2k\Delta \mathrm{t}\right),\sin \left(3k\Delta \mathrm{t}\right)\right]}^T\mathrm{m}{\mathrm{s}}^{-1}$ where $\circ$ is element-wise multiplication and $\Delta t$ is the sample time. Therefore, if ${\mathit{d}}^{\varphi }$ and ${\mathit{d}}^{\psi }$ specify that an axis is constrained, the velocity along that axis will be zero. Different $\left\{\varphi \right\}$ and $\left\{\psi \right\}$ then result in all constraints of Figure 2. The end effector twist ${\mathcal{V}}^b\left(k\right)$ was calculated using the transformation of Equation 2.

Poses $\mathit{T}\left(k\right)$ were obtained using the discrete-time version of Equation 1 for constant ${\mathcal{V}}^b\left(k\right)$ between samples (Lynch and Park, 2017):

$$\mathit{T}\left(k+1\right)=\exp \left({\left[{\mathcal{V}}^b\left(k\right)\right]}_{\times }\Delta t\right)\mathit{T}\left(k\right).$$ (9)

Discrete time ran for $5\mathrm{s}$ with sample time $\Delta t=0.1\mathrm{s}$ , an example is shown in Figure 3A. To mimic real world experiments, we introduced noise to $\mathit{T}\left(k\right)$ . Normally distributed noise with standard deviation ${\sigma }_p$ was added to the position $\mathit{p}\left(k\right)$ in $\mathit{T}\left(k\right)$ .

FIGURE 3.

(A) Constraint frame identification of a plane-plane constraint (Figure 2D) in simulation experiments. (B) Constraint frame identification of a revolute joint (Figure 2A) in robot experiments. (C) Reproduction of motion in a prismatic joint (Figure 2B) by applying force $\mathit{f}$ along the identified free axis of the constraint frame containing error $\Delta {\mathit{R}}^{\mathit{\psi }}$ , the reaction forces $\overline{\mathit{f}}$ are due to the real-world constraint. The setup in (B,C) can be configured to constrain specific axes. Measured end effector poses are shown as black dots with red-green-blue axes. Errors in the identified constraint frame geometry are shown enlarged in red with respect to the purple ground-truth geometry: perpendicular axis in (A), rotation axis in (B), and translation axis in (C).

Normally distributed noise with standard deviation ${\sigma }_R$ was added to the rotation vector representation $\mathit{\theta }\left(k\right)\in {\mathbb{R}}^3$ of $\mathit{R}\left(k\right)$ in $\mathit{T}\left(k\right)$ , related by ${\left[\mathit{\theta }\left(k\right)\right]}_{\times }=\ln \left(\mathit{R}\left(k\right)\right)$ . We assumed that the noise on position ${\sigma }_p$ and rotation ${\sigma }_R$ components were the result of one noise source with standard deviation $\sigma$ , such that ${\sigma }_p=\sigma$ and ${\sigma }_R=3\sigma {\mathrm{m}}^{-1}$ . Therefore, varying $\sigma$ affects both position and rotation components in the noisy poses $\stackrel{\sim }{\mathit{T}}\left(k\right)$ . The three-times larger effect of $\sigma$ on ${\sigma }_R$ was estimated by measuring noise on the robot pose in standstill.

5.2 Robot experiments

To test the method on real-world data, a Franka Research 3 (Franka Robotics, Munich, Germany) was used to collect constrained end effector poses $\stackrel{\sim }{\mathit{T}}\left(k\right)$ , an example is shown in Figure 3B. The experimenter physically moved the end effector through space in low-impedance guiding mode, which is part of the robot’s default control options, while the end effector was bolted to a constrained mechanical setup. While interacting with a user in the guiding mode, the robot complied with the Franka safety considerations. These consist of limits on all motor positions, velocities, accelerations, jerks, torques, and torque derivatives. The robot stops upon violation of these limits.

Poses $\stackrel{\sim }{\mathit{T}}\left(k\right)$ of five constraints (Figures 2A, B, D, I, K) were collected over ten trials per constraint for $5\mathrm{s}$ at $\Delta t=0.05\mathrm{s}$ . The experimenter attempted to track the poses of the simulations.

5.3 Error metrics for evaluation

Section 3.3 noted that the geometric parameters ${\mathit{R}}^{\varphi }$ , ${\mathit{R}}^{\psi }$ , and ${\mathit{p}}^{\psi }$ may contain redundancies depending on the constraints. Therefore, the error metrics reflect this.

Section 3.3 and Table 2 noted that ${\mathit{R}}^{\varphi }$ and ${\mathit{R}}^{\psi }$ either have none or one unique basis vector in ${\mathit{R}}^{\varphi }$ and none or one in ${\mathit{R}}^{\psi }$ . If there are no unique basis vectors, the error is irrelevant. If there is one unique basis vector $l$ , the error metric is the angle $\Delta R=\mathrm{acos}\left({\mathit{r}}_l·{\widehat{\mathit{r}}}_l\right)\mathbb{\in }\mathbb{R}$ between the ground-truth basis vector ${\mathit{r}}_l$ and the estimated basis vector ${\widehat{\mathit{r}}}_l$ . Angle $\Delta R$ was wrapped at $\pm 90\mathrm{deg}$ , as the positive and negative axes are irrelevant. For the error metric of ${\mathit{p}}^{\psi }$ , the Euclidian distance from the ground truth $\Delta p^{\psi }={‖{\mathit{p}}^{\psi }-{\widehat{\mathit{p}}}^{\psi }‖}_2$ was used. Only the unique components of ${\mathit{p}}^{\psi }-{\widehat{\mathit{p}}}^{\psi }$ , as shown in Table 2, were used.

The ground truth constraint frame geometries were ${\mathit{R}}^{\varphi }={\mathit{R}}^{\psi }={\mathit{I}}_{3\times 3}$ , with ${\mathit{p}}^{\psi }={\left[0.1,0.3,0.1\right]}^T\mathrm{m}$ for simulation, and ${\mathit{p}}^{\psi }={\left[0.0,0.0,0.244\right]}^T\mathrm{m}$ for robot experiments.

5.4 Implementation

Given experimental poses $\stackrel{\sim }{\mathit{T}}\left(k\right)$ , noisy twists ${\stackrel{\sim }{\mathcal{V}}}^b\left(k\right)$ were computed from Equation 9 by solving for ${\mathcal{V}}^b\left(k\right)$ . The optimizations of Equation 7 and Equation 8 were implemented in Python using NumPy and the SciPy 1.10 dual annealing global optimization algorithm with 100 maximum iterations and default settings otherwise (Xiang et al., 2013). Our code is available online ¹ .

The rotation matrices ${\mathit{R}}^{\varphi }$ , ${\mathit{R}}^{\psi }$ were parameterized by their rotation vector representations and bounded to $\pm 180\mathrm{deg}$ . The bounds on ${\mathit{p}}^{\psi }$ were $\pm 1\mathrm{m}$ . Initial guesses were randomized. For the simulation data, optimizations were done with added noise $\sigma \in \left[{10}^{-6},{10}^{-4}\right]\mathrm{m}$ spaced exponentially at 10 values. For the robot data, optimizations were done with no added noise $\sigma$ . One optimization of Equation 7 or Equation 8 for a single frame type took approximately $2\mathrm{s}$ on an Intel Core i5-6600 CPU with 8 GB RAM.

5.5 Simulation experiment results

The geometric errors ( $\Delta R^{\varphi }$ , $\Delta R^{\psi }$ , $\Delta p^{\psi }$ ) relate to noise ( $\sigma$ ) with approximately unit slope (Figure 4) for all constraints except for I, J, K, and L, whose errors ( $\Delta R^{\psi }$ , $\Delta p^{\psi }$ ) are constant with increasing noise. Constraints with fewer degrees of freedom are generally identified more accurately.

FIGURE 4.

Sensitivities of the geometric identification errors ( $\Delta R^{\varphi }$ , $\Delta R^{\psi }$ , $\Delta p^{\psi }$ ) to noise ( $\sigma$ ) in simulation experiments for all constraints of Figure 2.

5.6 Robot experiment results

Identifying constraint frames in demonstrations resulted in several candidate frame types (left, Table 4), each having associated degrees of freedom (middle, Table 4) and geometry (right, Table 4).

TABLE 4.

Identified constraint frames of robot demonstrations.

Figure 2 label	Min. norm $\left(7\right)\text{\hspace{0.17em}in\hspace{0.17em}frame\hspace{0.17em}type\hspace{0.17em}}\left\{\varphi \right\}\left(\text{mrad\hspace{0.17em}}{\mathrm{s}}^{-1}\right)$				Degrees of freedom*			Geometry error*
	Min. norm $\left(8\right)\text{\hspace{0.17em}in\hspace{0.17em}frame\hspace{0.17em}type\hspace{0.17em}}\left\{\psi \right\}\left(\text{mm\hspace{0.17em}}{\mathrm{s}}^{-1}\right)$
	$\left\{\mathcal{G}\right\}$		$\left\{\mathcal{B}\right\}$		${\mathbf{\lambda }}^{\mathbf{\varphi }}\left(\text{mrad\hspace{0.17em}}{\mathrm{s}}^{-1}\right)$			${\Delta R}^{\varphi }\left(\mathrm{deg}\right)$	$-$
	$\left\{\mathcal{B}\mathcal{B}\right\}$	$\left\{\mathcal{G}\mathcal{B}\right\}$	$\left\{\mathcal{G}\mathcal{G}\right\}$	$\left\{\mathcal{B}\mathcal{G}\right\}$	${\mathbf{\lambda }}^{\mathbf{\psi }}\left(\text{mm\hspace{0.17em}}{\mathrm{s}}^{-1}\right)$			${\Delta R}^{\psi }\left(\mathrm{deg}\right)$	$\Delta p^{\psi }\left(\text{mm}\right)$
A	$\mathbf{108.2}\pm \mathbf{7.9}$		$20.5\pm 10.8$		$37.9\pm 2.8$	$47.2\pm 3.8$	863.7 $\pm 46$ .7	$0.19\pm 0.02$ ‡	$-$
	$2.4\pm 0.2$	$3.6\pm 0.2$	$\mathbf{2.3}\pm \mathbf{0.1}$	$2.3\pm 0.2$	$1.6\pm 0.2$	$1.8\pm 0.2$	$4.5\pm 0.2$	$-$	$6.46\pm 0.21$
B	$47.5\pm 3.7$		$47.5\pm 3.7$		$39.0\pm 3.5$	$50.0\pm 3.8$	$55.3\pm 4.4$	$-$	$-$
	$12.9\pm 1.1$	$\mathbf{7.9}\pm \mathbf{0.5}$	$21.3\pm 1.5$	$23.3\pm 1.6$	$0.9\pm 0.1$	$1.8\pm 0.2$	256.5 $\pm 13.4$	$0.94\pm 0.18$ ‡	$-$
D	$144.3\pm 10.1$		$\mathbf{142.1}\pm \mathbf{9.2}$		$50.5\pm 3.7$	$60.8\pm 3.5$	958.2 $\pm 115.6$	$2.59\pm 0.50$	$-$
	$47.9\pm 7.2$	$\mathbf{35.1}\pm \mathbf{2.3}$	$60.3\pm 7.2$	$58.4\pm 8.1$	$1.4\pm 0.2$	156.4 $\pm 25.3$	185.7 $\pm 14.2$	$0.52\pm 0.13$	$-$
I	$\mathbf{85.4}\pm \mathbf{5.0}$		$86.3\pm 2.3$		$25.5\pm 1.7$	$30.3\pm 3.9$	786.5 $\pm 77.5$	$0.56\pm 0.15$	$-$
	$18.0\pm 1.9$	$\mathbf{10.6}\pm \mathbf{1.2}$	$23.1\pm 2.3$	$23.5\pm 2.1$	$2.4\pm 0.3$	$5.2\pm 0.9$	93.9 $\pm 7.3$	$1.64\pm 0.18$	$3.53\pm 1.12$ ‡
K	$\mathbf{428.3}\pm \mathbf{2.1}$		$495.3\pm 4.0$		260.2 $\pm 30.3$	430.8 $\pm 78.8$	716.1 $\pm 71.6$	$-$	$-$
	$54.2\pm 10.8$	$\mathbf{30.3}\pm \mathbf{4.9}$	$56.4\pm 8.1$	$59.8\pm 11.0$	$4.2\pm 1.4$	68.3 $\pm 17.8$	99.4 $\pm 14.6$	$2.45\pm 1.43$	$7.69\pm 2.32$

* In frame type with lowest minimized norm (bold). Italic: corresponds to ground-truth free axis. ^‡Error used in reproduction (Figure 5).

Section 4 noted that optimal frame types may be classified as the one with the lowest norms of Equation 7 or Equation 8. For constraints where any frame type is valid, norms are expected to be similar between frame types. This holds for all $\left\{\varphi \right\}$ and $\left\{\psi \right\}$ in A, but not for $\left\{\psi \right\}$ in B, D which have lowest norm in $\left\{\mathcal{G}\mathcal{B}\right\}$ . For constraints where only one frame type is valid ( $\left\{\mathcal{G}\mathcal{B}\right\}$ in I, K) the norms of the ground truth types are approximately half those of the other types. Therefore, frame types were classified correctly by the frame with the lowest minimized norm.

After classifying the optimal frame type, the associated degrees of freedom and geometry can be evaluated (middle, right, Table 4). If any threshold between $\left[60,260\right]\text{\hspace{0.17em}mrad\hspace{0.17em}}{\mathrm{s}}^{-1}$ is applied to all ${\mathit{\lambda }}^{\varphi }$ , and any between $\left[6,68\right]\text{\hspace{0.17em}mm\hspace{0.17em}}{\mathrm{s}}^{-1}$ to all ${\mathit{\lambda }}^{\psi }$ , the degrees of freedom of all five constraints (Table 2) are classified correctly. Suitable thresholds may be chosen at the midpoint of these ranges: $160\text{\hspace{0.17em}mrad\hspace{0.17em}}{\mathrm{s}}^{-1}$ for ${\mathit{\lambda }}^{\varphi }$ and 37 $\text{mm\hspace{0.17em}}{\mathrm{s}}^{-1}$ for ${\mathit{\lambda }}^{\psi }$ . These midpoint thresholds are at least 5 standard deviations away from the means of ${\mathit{\lambda }}^{\varphi }$ and ${\mathit{\lambda }}^{\psi }$ . The errors of the identified geometry are within $\Delta R^{\varphi }<1\mathrm{deg}$ , $\Delta R^{\psi }<2.5\mathrm{deg}$ and $\Delta p^{\psi }<8\text{\hspace{0.17em}mm}$ .

6 Application to robot task reproduction

This section illustrates how robot tasks can be reproduced using control expressed in the identified constraint frames of Section 4, and how their identification errors ( $\Delta R^{\varphi }$ , $\Delta R^{\psi }$ , $\Delta p^{\psi }$ ) may influence task performance. The Franka robot reproduced three straightforward endstop-to-endstop tasks involving constraints A, B, and I enforced by the mechanical setup from Section 5.2. An experiment is visualized in Figure 3C. We collected five trials for each task at a $0.05\mathrm{s}$ sample time and clipped each trial to $5\mathrm{s}$ .

6.1 Reproduction control

To reproduce the tasks, we used simple control based on the identified constraint frames from the experiments of Table 4, thereby illustrating simple LfD. We sent desired motor torques $\mathit{\tau }\in {\mathbb{R}}^7$ to the Franka torque controller based on constant desired wrenches (moments $\mathit{m}\in {\mathbb{R}}^3$ and forces $\mathit{f}\in {\mathbb{R}}^3$ ) expressed in the identified constraint frames. The torques and wrenches are related by $\mathit{\tau }={\mathit{J}}^T{\left[{\mathit{f}}^T{\mathit{m}}^T\right]}^T$ , where $\mathit{J}\in {\mathbb{R}}^{6\times 7}$ is the robot pose-dependent Jacobian (Lynch and Park, 2017).

Desired motor torques were sent to the robot using Robotic Operating System (ROS, version Noetic Ninjemys) from Ubuntu 20.04 with the franka_ros package. During operation, the user and bystanders were outside the workspace of the robot, and the user monitored the task reproduction with the robot’s emergency stop in hand.

Desired wrenches were set to constant values ( $3.5\text{\hspace{0.17em}Nm}$ and $7\mathrm{N}$ ) along the free axes of the identified constraint frames, and to zero ( $0\text{\hspace{0.17em}Nm}$ and $0\mathrm{N}$ ) along the constrained axes. Therefore, the control for constrained task A applied a pure moment using the identified ${\mathit{R}}^{\varphi }$ . The control for constrained task B applied a pure force using the identified ${\mathit{R}}^{\psi }$ . The control for constrained task I applied a moment and a force using the identified ${\mathit{R}}^{\varphi }$ , ${\mathit{R}}^{\psi }$ , and ${\mathit{p}}^{\psi }$ , but for this experiment we set $\Delta R^{\varphi }=\Delta R^{\psi }=0$ to isolate the effect of $\Delta p^{\psi }$ .

6.2 Effects of identification accuracy

During constrained task reproduction there will be reaction wrenches along the constrained axes, which we consider undesirable in these experiments. Such reaction wrenches can be caused by, among other factors, the control applying wrenches based on imperfect constraint identification. Therefore, we compared the reaction wrenches in case the constraint frames used in the controller contain no errors ( $\Delta R^{\varphi }=\Delta R^{\psi }=\Delta p^{\psi }=0$ ), identification errors from methods in the literature that use prior knowledge (De Schutter et al., 1999; Niekum et al., 2015), and identification errors from our method without prior knowledge of the constraint (Table 4).

From the measured interaction wrenches ( $\mathit{m}\left(k\right)$ and $\mathit{f}\left(k\right)$ ) we determined the reaction wrenches (moments $\overline{\mathit{m}}\left(k\right)$ and forces $\overline{\mathit{f}}\left(k\right)$ ) along the ground-truth constrained axes (Figure 2), and determined their peak magnitudes ${\overline{m}}^{*}$ and ${\overline{f}}^{*}$ with ${\overline{m}}^{*}=\max {‖\overline{\mathit{m}}\left(k\right)‖}_2\mathbb{\in }\mathbb{R}$ . Thereby, we obtain a metric of how constraint identification accuracy influences undesirable reaction wrenches in task reproduction. The results are shown in Figure 5. Larger identification errors (i.e., from our method) were expected to cause larger reaction wrenches, but this is not necessarily the case. In all reproductions, the physical endstops of the setup were reached.

FIGURE 5.

The peak magnitude reaction wrenches ( ${\overline{f}}^{*}$ , ${\overline{m}}^{*}$ ) and their means and standard deviations over five reproductions of the tasks containing constraints (A–C). Colors represent the source of the errors in the constraint frame used in the control.

7 Discussion and conclusion

This work identifies constraints from kinematic data using constraint frames, consisting of a frame type that determines what body the frame is attached to, geometry that determines the orientation and position, and the degrees of freedom of velocities in that frame. First, frame geometries are identified by minimizing a norm on velocities in all frame types. Second, the optimal frame type is classified as the one with the lowest norm. Third, the degrees of freedom can be classified by thresholding the velocities in that frame.

7.1 Advantages

The method does not require force measurements, and can therefore be applied to any system that measures positions and orientations of constrained objects or manipulators. Examples include other (mobile) robotic arms with different kinematic chains, humanoid robots, and mobile robots constrained by their environments. Besides robotics, the method may also be applied to motion tracking systems, for example, to monitor human movement or estimate human joints (Ancillao et al., 2022). While impedance control was used to collect pose data and reproduce tasks, the constraint identification itself does not depend on force measurements. Because force measurements can improve identification accuracy it is the topic of future work (De Schutter et al., 1999; Subramani et al., 2020). Moreover, correct kinematics-based constraint identification requires that all true degrees of freedom are excited. Otherwise, there is no discernible difference between, e.g., a prismatic joint constraint and unconstrained but straight line translation, which will have the same apparent degrees of freedom.

The method requires no specific prior constraint models, nor estimates of geometry. Hence, there is no need to manually estimate or define constraints, which may be challenging to do exhaustively for all constraints that can be encountered in everyday settings. The method can identify a wide variety of constraints, of which twelve common examples were shown. However, many more can be identified by enumerating over all possibilities, including constraints where the angular and linear velocity frames are not orthogonal.

The method identifies a fixed number of intuitive parameters (Table 1). Methods that fit specific constraint models must identify and choose from multiple candidate models, each with their own parameters. Therefore, our method may be more efficient when many different constraints must be considered, such as in everyday settings.

Our identified constraint frames can be directly interpreted as “task frames” from the literature since both frames conveniently describe contact tasks (Bruyninckx and De Schutter, 1996; Mousavi Mohammadi et al., 2024). Although recent work on task frame identification by Mousavi Mohammadi et al. (2024) also has the above advantages, in this work we also classify constraints by discerning the degrees of freedom associated with our frames. Although we reproduced simple tasks, such frames can also be used to reproduce more complicated contact tasks with hybrid position and force control applied to the task frame axes (Conkey and Hermans, 2019; Ureche et al., 2015). Alternatively, impedance controller stiffness may be varied based on the identified constraints.

7.2 Limitations and future work

In this work, we applied our method to a window of kinematic data which we assume contains a single constraint. In future work, we will investigate how our method can be applied to data containing sequential constraints. Furthermore, we only considered single-contact constraints which can be modeled using a single constraint frame. Multiple-contact constraints, such as the two ends of a stick contacting separate planes, cannot be fully modeled using a single instance of the current method, since only one of the two contact points will be identified. However, a second instance of our method may identify a second contact point, if the second solution is (forced to be) distinct. Therefore, multiple contacts may be identified with multiple instances of our method, which is the topic of future work.

We defined constraint identification as a minimization problem and applied a general global-local optimization method. Improvements in accuracy and time efficiency may be achieved in several ways: by more efficient optimizer implementations, by choosing a different optimization method suited for our specific problem, or by tuning the optimization parameters. Furthermore, prior information may be useful for initialization. For example, a vision system may be used to identify a tool tip as prior information on constraint geometry.

Identifying constraint geometry does not require any parameter choice, but classifying the degrees of freedom requires two thresholds on RMS velocities, which can be chosen heuristically or empirically. Heuristically, thresholds may be chosen based on prior information, such as expected noise and expected velocities, which may differ between applications and measurement systems. If such prior information is available, classification may perform as expected without the need for threshold tuning. Empirically, thresholds may be chosen following demonstrations with known ground-truth constraints (Section 5). If such demonstrations are available and representative of future tasks, no explicit prior information about noise and velocities are needed, which may be more convenient depending on the application.

Geometric identification errors were found to scale linearly with noise in simulation experiments, and the exact scaling varies with the underlying constraints. Errors in robot experiments were larger than those in simulation experiments, which may be due to unmodeled factors such as structural compliance. Classification of the ground-truth constraint frame type was successful in all robot experiments. An analysis of factors that influence identification and classification performance is outside the scope of this work, such as the constraint, the optimizer, the relative direction and magnitude of motion, and the sample size.

Constraint identification methods in the literature report geometric identification accuracies using different metrics. In our method, we report the geometric parameter errors between points, lines, and planes. Methods that use similar metrics report similar errors: within 1–7.5 mm and 0.5–5 deg for point-on-plane contacts, prismatic, and revolute joints (De Schutter et al., 1999; Sturm et al., 2011; Mousavi Mohammadi et al., 2024). Other methods report the fitness between observations and their candidate constraint models (Sturm et al., 2011; Subramani et al., 2018; Subramani et al., 2020; van der Walt et al., 2025). Of such methods, the most accurate report sub-millimeter mean fit errors (Subramani et al., 2018). While such sub-millimeter mean model fit errors are not directly comparable to geometric parameter errors, they may correspond to sub-millimeter geometric errors.

While our method may be more versatile, it may yield larger geometric identification errors than methods that fit specific constraint models to data. Regardless, such larger errors (e.g., 5 mm instead of 0.5 mm) did not lead to substantially larger reaction wrenches in simple reproduction experiments with the default Franka controller (Section 6). Furthermore, similar errors have been shown to lead to acceptable performance in more complicated tasks in related work (Mousavi Mohammadi et al., 2024). The obtained accuracy may therefore be sufficient for such tasks, and other factors may have a greater effect on task performance, such as the task definition, environment, robot, controller, and definition of success. For example, control methods that are designed for compliant contact, such as impedance control, may be more robust to misidentified constraint geometry than non-compliant control. We aim to apply such compliant control methods to reproduce constrained tasks through LfD in future work.

7.3 Conclusion

This work identified constraint frames in robot tasks without prior knowledge of the constraints or tasks and without force measurements. Automatically modeling such robot-environment interactions, for example, in the context of Learning from Demonstration, may support versatile autonomous robot applications.

Funding Statement

The author(s) declare that financial support was received for the research and/or publication of this article. This research was funded by the Dutch Ministry of Education, Culture, and Science through the Sectorplan Bèta en Techniek.

Data availability statement

The raw data supporting the conclusions of this article will be made available by the authors, without undue reservation.

Author contributions

AO: Conceptualization, Data curation, Formal Analysis, Investigation, Methodology, Project administration, Software, Supervision, Validation, Visualization, Writing – original draft, Writing – review and editing. DD: Supervision, Writing – review and editing. HvdK: Funding acquisition, Resources, Supervision, Writing – review and editing. MV: Conceptualization, Methodology, Project administration, Resources, Supervision, Writing – review and editing.

Conflict of interest

The authors declare that the research was conducted in the absence of any commercial or financial relationships that could be construed as a potential conflict of interest.

Generative AI statement

The author(s) declare that no Generative AI was used in the creation of this manuscript.

Publisher’s note

All claims expressed in this article are solely those of the authors and do not necessarily represent those of their affiliated organizations, or those of the publisher, the editors and the reviewers. Any product that may be evaluated in this article, or claim that may be made by its manufacturer, is not guaranteed or endorsed by the publisher.