Extended Diffeomorphism for Real-Time Motion Replication in Workspaces with Different Spatial Arrangements

TL;DR

This paper introduces extended diffeomorphism techniques to enable real-time, smooth motion replication across robots in different spatial arrangements, enhancing teleoperation efficiency.

Contribution

It proposes novel extended diffeomorphism designs for compensating spatial workspace differences, improving motion accuracy and smoothness in robot teleoperation.

Findings

Reduced mapping errors in target keypoints

Achieved smooth and precise motion replication

Demonstrated effectiveness with dual-arm UR5 robot

Abstract

This paper presents two types of extended diffeomorphism designs to compensate for spatial placement differences between robot workspaces. Teleoperation of multiple robots is attracting attention to expand the utilization of the robot embodiment. Real-time reproduction of robot motion would facilitate the efficient execution of similar tasks by multiple robots. A challenge in the motion reproduction is compensating for the spatial arrangement errors of target keypoints in robot workspaces. This paper proposes a methodology for smooth mappings that transform primary robot poses into follower robot poses based on the predefined key points in each workspace. Through a picking task experiment using a dual-arm UR5 robot, this study demonstrates that the proposed mapping generation method can balance lower mapping errors for precise operation and lower mapping gradients for smooth replicated movement.

Tables

Table 1. TABLE I: Parameters of the Twisted Affine Transformation

Parameter	Meaning	Range
$a_x,a_y,a_z$	Scaling along $x$, $y$, $z$ axes	$[0.5,2]$
$s_{xy},s_{xz},s_{yz}$	Shear in $xy$, $xz$, $yz$ planes	$[0,1]$
$\varphi ,\theta ,\psi$	Rotation around $x$, $y$, $z$ axes	$[-\frac{\pi }{2},\frac{\pi }{2}]$
$r_x,r_y,r_z$	Reflection along $x$, $y$, $z$ axes	$\{-1,1\}$
${\varphi }_t,{\theta }_t$	Twist angles around $x$, $y$ axes	$\{0,\pi \}$
$t_x,t_y,t_z$	Translation vector	$--$

Table 2. TABLE II: Evaluation metrics in Simulation 2

	(a) DIFF	(b) R-DIFF	(c) TA-DIFF
Positional Error (Max)	145 mm	211 mm	4.73 mm
Success Rate	90 %	87 %	100 %
RBF Iteration (Average)	39	29	10

Table 3. TABLE III: Target positions in each environment

	Right Arm	Left Arm
Experiment 1	Preset Values	Preset Values
Target 1 $\bullet$	$(0.50,0.20,1.20)$	$(0.79,0.48,1.02)$
Target 2 $\bullet$	$(0.70,0.20,1.20)$	$(0.55,0.16,1.22)$
Target 3 $\bullet$	$(0.70,0.40,1.20)$	$(0.45,0.41,1.26)$
Target 4 $\bullet$	$(0.50,0.40,1.20)$	$(0.68,0.24,1.02)$
Experiment 2	Detected (AprilTag)	Detected (AprilTag)
Target 1 $\bullet$	$(0.492,0.502,0.898)$	$(0.746,0.344,1.061)$
Target 2 $\bullet$	$(0.488,0.163,0.919)$	$(0.482,0.334,0.912)$
Target 3 $\bullet$	$(0.721,0.164,0.927)$	$(0.849,0.146,0.961)$
Target 4 $\bullet$	$(0.756,0.506,0.905)$	$(0.564,0.059,0.896)$

Table 4. TABLE IV: Evaluation metrics in Experiment 1

	(a) DIFF	(b) R-DIFF	(c) TA-DIFF
Positional Error	3.26 mm	3.36 mm	2.94 mm
Maximum Gradient	44.13	5.41	5.39
RBF Iteration	88	28	4

Equations

$$\hat{\mathbf{p}}=\hat{\mathbf{\Phi }}(\mathbf{p}),\hat{\mathbf{q}}=\hat{\mathbf{\Psi }}(\mathbf{p},\mathbf{q}),$$

$${\hat{\mathbf{\Phi }}}_{\mathrm{DIFF}}(\mathbf{p})$$

$${\hat{\mathbf{\Psi }}}_{\mathrm{DIFF}}(\mathbf{p},\mathbf{q})$$

$$\mathbf{\Phi }(\mathbf{p})$$

$$\mathbf{\Psi }(\mathbf{p},\mathbf{q})$$

$$k_f(\mathbf{p}|\rho ,\mathbf{c})=\exp (-{\rho }^2||\mathbf{p}-\mathbf{c}|{|}^2),$$

$$0<{\rho }_1<\frac{1}{\sqrt{2}||{\mathbf{v}}_1||}{e}^{\frac{1}{2}}.$$

$$\mathbf{\Phi }(\mathbf{p})$$

$$F_{\Phi }({\rho }_{1,j},{\rho }_{3,j},{\mathbf{v}}_{1,j},{\mathbf{v}}_{3,j})$$

$$F_{\Psi }({\rho }_{2,j},{\mathbf{v}}_{2,j})$$

$$\hat{\mathbf{p}}$$

$${\varphi }_t(x)$$

$${\hat{\mathbf{\Phi }}}_{\mathrm{TA}}(\mathbf{p})$$

$$R_{\text{TA}}$$

$$\cdot R_{\text{rotation}}(\varphi ,\theta ,\psi )\cdot R_{\text{reflection}}(r_x,r_y,r_z)$$

$$\cdot R_{\text{twist}}({\varphi }_t(x),{\theta }_t(y)),$$

$${\mathbf{t}}_{\text{TA}}$$

$$F_{\mathrm{TA}}$$

$${\dot{\mathbf{x}}}_i=k_p({\mathbf{x}}_i-{\bar{\mathbf{x}}}_i),$$

$$\underset{{\dot{\mathbf{\theta }}}_i}{\min }\sum _i^N$$

$$R_{\text{scaling}}(a_x,a_y,a_z)=\mathrm{diag}(a_x,a_y,a_z)$$

$$R_{\text{shear}}(s_{xy},s_{xz},s_{yz})=\left[\begin{array}{ccc}1& s_{xy}& s_{xz}\\ 0& 1& s_{yz}\\ 0& 0& 1\end{array}\right]$$

$$R_{\text{reflection}}(r_x,r_y,r_z)=\mathrm{diag}({\mathrm{r}}_{\mathrm{x}},{\mathrm{r}}_{\mathrm{y}},{\mathrm{r}}_{\mathrm{z}})$$

$$R_{\text{rotation}}(\varphi ,\theta ,\psi )=R_{\text{yaw}}(\psi )\cdot R_{\text{pitch}}(\theta )\cdot R_{\text{roll}}(\varphi )$$

$$R_{\text{twist}}({\varphi }_t(x),{\theta }_t(y))=P\cdot R_{\text{pitch}}({\theta }_t(y))\cdot P\cdot R_{\text{roll}}({\varphi }_t(x))+(I-P)$$

$$P=\mathrm{diag}(1,1,0)$$

Full text

I INTRODUCTION

Robotic teleoperation plays an important role in hazardous environments and in areas that are inaccessible to humans. A fundamental functionality of a teleoperation system is to map the operator’s input to the robot’s movement. When a single operator controls multiple robots to perform a collaborative task, such as lifting a large object, the operator needs to repeat similar input to control each robot in the non-common action phase, e.g., guiding individual robot to the corresponding grasping point. Once the geometric errors among the robots become acceptable, the operator can send a common command to control all the robots simultaneously, e.g., lifting the table up.

Although multi-robot teleoperation is a promising method to expand the usage of robot embodiment, large workloads are required for precise control in the non-common action phase. Therefore, a reduction method of operational workload is important for practical multi-robot teleoperation. One approach is to develop an interface to easily select a target robot in the non-common action phase and a shared control surface in the common action phase[1]. While this approach reduces the operation workload for selecting robots, the operator still needs to control robots individually, which would lead to a decrease in work efficiency. In this study, we address a methodology to control multiple robots simultaneously to improve the efficiency in the non-common action phase. This paper tackles on real-time replication of a primary robot action for the other robots, considering the geometric difference among the robots’ workspaces. This approach aims to allow the operator to focus on and control a primary robot, and the other robots automatically adjust the geometric errors and reach the target poses without time delay.

A notable example of non-real-time motion replication is imitation learning (IL). IL is an effective method for generating the movements of a robot in a replicated environment from the movements of a reference robot. For instance, Dynamic Movement Primitives (DMP) can extract the essence of the robot’s motion from a single human demonstration and adjust it in another workspace[2]. In situations where a user operates multiple robots, IL is not suitable because the goal pose is not given in advance.

There have been several studies on real-time motion replication for simultaneous multi-robot operation. For instance, variable scaling methods [4, 5] are used to adjust motion scaling factors for each robot to absorb geometric error in the corresponding environment. However, such scaling methods require updating the scaling coefficients when the geometric configuration of the workspace changes, leading to a discontinuous and non-transparent experience. To overcome the shortcomings of the scaling methods, a diffeomorphism-based spatial mapping method has been proposed [6]. The map is represented by a weighted composition function of radial basis functions (RBF), which is continuous and differentiable, and designed for minimizing the inter-environmental errors of position and orientation between the robots. However, in highly unstructured environments, the author reported that significant distortion occurs in the mapping due to limited compensation for large inter-environmental errors. Since excessive distortion in positional mapping causes a large robot velocity, the gradient of mapping needs to be low in practice. Nevertheless, the conventional methods are hard to achieve both a lower mapping gradient and a lower mapping error in some cases.

This paper aims to define a novel diffeomorphism to minimize both mapping gradient and mapping error, even in cases of highly geometric randomized workspaces. We propose two types of extended diffeomorphisms: Rotation Extended Diffeomorphism and Twisted Affine Extended Diffeomorphism. Rotation Extended Diffeomorphism is an extension of vector-based positional diffeomorphism to quaternion-based one. Twisted Affine Extended Diffeomorphism is an extension of the conventional method with the addition of a non-linear mapping for workspaces with various deformations. Fig. 1 shows a concept image of our proposed mapping methodology. We demonstrate the effectiveness of the presented methodology through simulation and experiments.

II Diffeomorphism for Motion Replication

We define a mapping formulation for real-time motion replication. The transformation from the primary robot position $\bm{p}$ and orientation $\bm{q}$ to another robot’s position $\hat{\bm{p}}$ and orientation $\hat{\bm{q}}$ is represented as follows:

[TABLE]

where $\hat{\bm{\Phi}}$ and $\hat{\bm{\Psi}}$ are diffeomorphic mapping functions for position and orientation, respectively. This paper address a novel design of the position mapping function ${\bm{\hat{\Phi}}}$.

II-A Conventional Diffeomorphism

According to the prior work [6], we here define conventional diffeomorphic mapping functions $\hat{\bm{\Phi}}$ and $\hat{\bm{\Psi}}$ in eq. (1) as $\hat{\bm{\Phi}}_{\rm DIFF}$ and $\hat{\bm{\Psi}}_{\rm DIFF}$, which are written as:

[TABLE]

where $\bm{\Phi}_{j}$ and $\bm{\Psi}_{j}$ ($j=1\cdots J$) are $j$-th basic composition function for position and orientation mapping, and $J$ is the total number of the composition. Note that the forms of the basic composition functions are unique, and we omit the $j$ index for a simple expression. The basic composition functions ($\bm{\Phi}(\bm{p})$ and $\bm{\Psi}(\bm{p},\bm{q})$) are defined as:

[TABLE]

where $\bm{v}_{1}\in\mathbb{R}^{3}$ and $\bm{v}_{2}\in\mathbb{H}$ are corresponding to the amount of linear transformation of key points in position and orientation. The transformation is weighted by RBF, which is represented by

[TABLE]

where $\rho\in\mathbb{R}^{1}$ and $\bm{c}\in\mathbb{R}^{3}$ are variables determining the shape of the function, corresponding to the slope and center position of RBF, respectively. The previous study in [6] presented a sufficient condition for diffeomorphism as:

[TABLE]

While conventional mapping can absorb translational error, it is not compatible with workspace rotation and reflection cases. That is because the optimization parameter ${\bm{v}_{1}}$ is used for a locally weighted shift. In this paper, we present two extended diffeomorphisms to address the rotation, reflection, and swapping of key points in workspaces.

II-B Rotation Extended Diffeomorphism

II-B1 Orbital-Rotation for Error Representation

In order to improve the compatibility of the conventional diffeomorphic mapping with rotated situations, we redefine the geometric error formulation among the workspaces. Fig. 2 shows the error formulation when key point A shifts and rotates as key point A’. The conventional method shown in Fig. 2(a) represents the geometric error as translation and spin-rotation terms. Since consideration of the rotation of the key points group is not sufficient, we additionally consider the orbital-rotation term for position error, which provides a novel form of diffeomorphism compatible with orbital-rotational error as shown in Fig. 3. In the mathematical expression, we define the diffeomorphic function $\hat{\bm{\Phi}}$ in eq. (1) as $\hat{\bm{\Phi}}_{\rm R-DIFF}$ and modify the basic composition function for position in eq. (4) as follows:

[TABLE]

where $\bm{p^{q}}$ is a pure quaternion form of ${\bm{p}}$. ${\bm{v}_{3}}$ is a quaternion representing the orbital-rotational error of key points.

II-B2 Parameter Optimization

We present an algorithm for determining the parameters of our diffeomorphic mapping as shown in Algorithm 1. The sets of variable parameters for position and orientation mapping are composed of ${\rho_{1,j},\bm{c}_{1,j},\bm{v}_{1,j},\rho_{3,j},\bm{c}_{3,j},\bm{v}_{3,j}}$ ($j=1\cdots J$) and ${\rho_{2,j},\bm{c}_{2,j},\bm{v}_{2,j}}$ ($j=1\cdots J$), respectively. In the optimization process, parameters ${\bm{c},\bm{v}}$ are selected based on a geometric relationship. After deciding ${\bm{c},\bm{v}}$, a parameter ${\rho}$ is selected to minimize the following cost functions:

[TABLE]

where $\bm{p}_{k,{\rm goal}}$ denotes the goal position of key points, $\bm{q}_{k,{\rm goal}}$ denotes the goal orientation of key points, and $\bm{r}_{k,{\rm goal}}$ denotes the goal orbital-rotation of key points. $k$ is the index of key points. ${\bm{q}_{0}}$ is a null rotation quaternion. The composition iteration of RBF is terminated when the position and orientation errors fall below a predefined minimum threshold, or when the number of compositions $j$ reaches the maximum $J_{max}$.

II-C Twisted Affine Extended Diffeomorphism

II-C1 Twisted Affine Transformation

In this paper, assuming that there are four key points, we present a twist transformation to address key points swapping. For instance, the twist transformation of the roll axis can be defined using a rotation matrix in the roll direction ${R_{\text{roll}}}$ as:

[TABLE]

where the twist angle ${\phi_{t}(x)}$ is a sigmoid function, which can be written as:

[TABLE]

where ${\rho_{\mathrm{sig}}}$ is a parameter of the intensity of the twist. The result of applying the twist transformation of the roll axis to the xy-plane is shown in Fig. 4. In this paper, the twist transformation is defined in roll and pitch directions.

By combining the twist transformation with the affine transformation, the mapping can address key points’ scaling, rotation, reflection, and swapping simultaneously. We present the mapping method as Twisted Affine Extended Diffeomorphism, written as:

[TABLE]

where

[TABLE]

The definitions of each rotation matrix are provided in the Appendix. The parameters of the Twisted Affine mapping are shown in Table I.

II-C2 Parameter Optimization

Algorithm 2 shows the optimization process. The discrete parameters ${\bm{B}}$ are selected in order. The continuous parameters ${\bm{A}}$ are selected with the BFGS method to minimize the following cost functions:

[TABLE]

III Manipulator Control

We simultaneously control multiple robots, which are placed in geometrically randomized workspaces, using our presented motion replication method. We input a target end-effector pose of the primary robot using a teleoperation interface. The other robots follow transformed target poses calculated from eq. (1). Then we obtain an augmented variable $\bm{x}_{i}$ of the target pose of $i$-th robot as $\bm{x}_{i}=[\bm{p}_{i}^{T}\ \bm{q}_{i}^{T}]^{T}$

The robot controller to follow the target poses is implemented using OpenHRC [7]. We apply a proportional control shown in eq. (18) and use its output as target end-effector velocities.

[TABLE]

where $\bar{\bm{x}}_{i}$ denotes the current pose of the $i$-th robot. ${k_{p}=2.0}$ is the proportional gain. The inverse kinematics that converts the desired end-effector velocity to the desired joint velocity is formulated as a QP (quadratic programming) optimization problem as shown in eq. (19).

[TABLE]

where $N$ is the number of robots. $\bm{\theta}_{i}$ and ${\bm{J}_{i}(\bm{\theta})}$ is the joint angle and Jacobian matrix of $i$-th robot, respectively. The obtained joint velocity is sent to the robot via a Joint Velocity Controller.

IV Simulation

IV-1 Simulation 1

We conduct numerical simulations to compare the conventional Diffeomorphism (DIFF) and the proposed Rotation Extended Diffeomorphism (R-DIFF). In this simulation, multiple sub-key points around target objects are added to improve the consistency of geometric information around targets. The number of target objects $K$ is set to 4. Sub key points around each target object are placed from a distance of 0.05 m in x,y,z directions. The total number of sub-key points ${K_{s}}$ is 27. All points, including target objects and sub-key points around target objects, are used as key points ${\bm{P}_{\mathrm{inits}},\bm{P}_{\mathrm{goal}}}$ in Algorithm 1. The number of replicated environments is 3. The maximum number of RBF iterations $J_{max}$ is set to 100. Iteration ends when eq. (9) is under 5 mm.

Fig. 5 shows the results of simulation 1. In the case of DIFF, a steep gradient and positional error are confirmed in Planes 2 and 3. However, in the case of R-DIFF, the gradient is gentle and the positional error is nearly zero. In Plane 3, the positional error of the key points is 18.65 mm (DIFF), 5.58 mm (R-DIFF).

IV-2 Simulation 2

Simulation 2 examines the reliability of the mapping in various situations. DIFF, R-DIFF, and Twisted Affine Diffeomorphism (TA-DIFF) are compared in the situation where target objects are placed randomly within the range ${\bm{p}_{k,{\rm goal}}\in[0.1,0.6]^{3}}$. There are no sub-key points around target objects ${(K_{s}=0)}$. The number of replicated environments $N$ is 100. The other conditions are the same as ones in simulation 1.

Fig. 6 is the boxplot showing the maximum gradient and positional error between target objects. The maximum gradient is calculated by the spectral norm of the Jacobian matrix for each grid point set at 0.02 m intervals on the map. The positional error between target objects is calculated using the value of eq. (9) at the end of the mapping algorithm. From the results, the proposed method was confirmed to generate smoother mappings with smaller velocity gradients, taking into account the geometric relationships between target objects. Regarding positional errors, TA-DIFF was confirmed to be less than 5 mm in all situations, but R-DIFF showed larger positional errors. This is thought to be because eq.  (8) does not provide appropriate compensation in some cases. Table II summarizes the other evaluation values for each method. In DIFF and R-DIFF, there were approximately 10 % cases where the error did not fall below the desired value after 100 RBF superpositions. However, the proposed TA-DIFF guaranteed that the position error in the mapped environment would always fall below the target minimum error under all conditions.

V Experiment

V-A Setup

To verify the effectiveness of the proposed method, we conduct experiments using a dual-arm UR5 (Universal Robots) with 2F-85 grippers (Robotiq). A ROS2 velocity controller is implemented for the robot. The experimental setup of the dual-arm UR5 robot is shown in Fig. 7. The robot is connected to a PC running Ubuntu 22.04 with a PREEMPT_RT patched kernel. The right arm is offset in the $y$-direction by +0.6 m relative to the left arm.

Table III lists the target positions used in each experiment. In Experiment 2, red, yellow, green, and blue blocks are used as target objects representing the robot’s environment. Their positions and orientations are detected using AprilTag (AR marker) and a ZED 2i stereo camera. We employ a Leap Motion Controller 2.

V-B Experiment 1

In Experiment 1, the primary (left) robot follows a trajectory in the order: Target 1 $\to$ 2 $\to$ 3 $\to$ 4 $\to$ 1 $\to$ 3 $\to$ 2 $\to$ 4. We assess the responses of the follower (right) robot under the following mapping methods: DIFF, R-DIFF, and TA-DIFF.

Fig. 9 shows the command and response position of the end-effector, and the velocity norm for each mapping. In DIFF, oscillatory behavior was observed in the position response when moving from Target 4 to 1. Besides, the generated motion in DIFF environment included a trajectory with a larger velocity norm than in the original environment. In contrast, with the proposed R-DIFF and TA-DIFF, the velocity deviation from the original trajectory was small, and the position response remained smooth. The evaluation indexes for each mapping are shown in Table IV. The proposed mappings significantly reduced the velocity gradient while achieving the required positional accuracy.

V-C Experiment 2

In Experiment 2, the operator conducts a task grasping Targets 1–4 and moving them to the location of Target 2 in both the primary and follower environment at the same time, as shown in Fig. 8. The pinch and grab gestures of the operator’s hand are used as triggers to activate the robot operation and open/close the mounted gripper, respectively. The operator conducts the task three times.

In the case of DIFF, the follower robot sometimes moved very quickly even though the commanded velocity for the primary robot was not so large. This caused an emergency stop because the follower robot reached its safety limiter. In the case of R-DIFF, no emergency stop of the follower robot occurred. However, the prediction of the movement of the follower robot would be difficult because the shape of the map is not intuitive to the human operator, which prolongs the task completion. In the case of TA-DIFF, thanks to the smooth and intuitive shape of the mapping, the operator was able to grasp all targets.

VI Discussion

The proposed R-DIFF is compatible with an infinite number of key points theoretically, which is advantageous when adding multiple sub-key points around target objects, as in simulation 1. Compared to DIFF, R-DIFF tends to improve the mapping gradient, but in some cases, the mapping error is higher than DIFF. TA-DIFF performs the best in terms of mapping error and mapping gradient, but since it assumes that the number of key points is four, further consideration is needed for more than 5 key points.

VII Conclusion

In this paper, we presented the Rotation Extended Diffeomorphism and the Twisted Affine Extended Diffeomorphism, and their application as a mapping method for real-time motion replication. Through simulation, our approach is more compatible than the conventional method in terms of mapping gradient and mapping error. The experimental results show that the proposed mappings enabled smooth position mappings for robots in the replicated environment, while maintaining the smallest velocity deviation from a primary robot. These findings indicate that the proposed method would be effective for real-time motion reproduction in multi-robot teleoperation. Future work will address mappings at levels of velocity and acceleration to facilitate smooth environmental interaction of each robot.

APPENDIX

[TABLE]

[TABLE]

[TABLE]

[TABLE]

[TABLE]

[TABLE]