## Starting from Rope Straightening…

## Introduction

Years ago the algorithm TPS-RPM was proposed by Haili Chui to tackle the problem of matching point clouds of an object viewed from different directions. This algorithm proves useful in robotic trajectory planning by human demonstration, where the points of the actual object is matched with training object. Then the same transfer function is used to warp the training trajectory, resulting in a desirable trajectory for the actual object.

However, the application of TPS-RPM in this scenario is limited due to its discrete nature. In reality an object, say a rope, consists of consecutive points. The point cloud is only a sampling result of all points on the rope. The distance between two adjacent points should always remain the same. However, TPS-RPM is not designed to meet this condition. As a result, it may result in a trajectory that under-stretches or overstreches the object.

To deal with this problem, we formerly introduced a new algorithm TSM-RPM. It is further found that many similar algorithms can be modified in the same manner to preserve the distances between adjacent points. In the following sections I will explain the method in detail.

## Showcase

Below are two videos showing the performance of **TPS-RPM** (thin-plate spline - robust point matching) algorithm and **TSM-RPM** (tangent space mapping - robust point matching) algorithm, respectively.

As we can see, the traditional TPS-RPM algorithm **fails** to address the problem of rope straightening.

TSM-RPM, on the other hand, is able to generate a trajectory that perfectly straightens a rope, following human demonstration.

## TSM-RPM Algorithm

The secret of planning a trajectory that preserves the distance between adjacent points lies in tangent space.

In TPS-RPM algorithm, point-registration is performed on the training rope to align them as close to the testing rope as possible. Then the same function is used for each critical point on the robot’s trajectory. This algorithm performs well in a lot of scenarios, including rope knotting and cloth folding. However, there is a major limitation.

For the ease of explanation, here we take rope manipulation on a plane as an example. As shown in the figure below, the rope can be equivalently represented in the Cartesian space and in the tangent space. (a) and (c) show the rope at training scene and at test scene in the Cartesian space. (b) and (d) are the tangent graphs of the rope, where the horizontal axis is the arc length along the rope, and the vertical axis is the direction of the unit tangent vector. In this case, the rope’s unit tangent vector is one dimensional.

During training, the rope is manipulated from the initial shape to the final shape. This training procedure can be decomposed into several snapshots at different time frames. At each time frame, a tangent graph of the rope can be constructed (see (b)). At test time, the rope starts with a different initial shape and consequently, a new initial tangent graph. A transformation function $f_{TSM}$ could be found in the tangent space that maps the initial tangent graph at training to the initial tangent graph at test. That same function $f_{TSM}$ can be utilized to warp the tangent graphs at training to get the corresponding tangent graphs at test in subsequent time frames.

After getting the tangent information of the rope at test, the tangents are integrated along the arc length to convert the tangent information into position information in order to get the manipulation trajectory that robot should follow at test time.

## Other Algorithms

TSM-RPM is equivalent to using TPS-RPM in tangent space instead of in cartesian space. This being said, we can also apply other common point matching algorithms in tangent space.

CPD, for example, is another point matching algorithm. Unlike TPS-RPM, CPD does not directly give us the corresponding matrix, thus we will have to calculate the corresponding matrix on our own, or, for each point on the testing rope, find the most relevent point on the training rope. Once the correspondence is established, we can use the weighted summation to calculate the desired tangent curve in the test scene.

**In summary, for a given algorithm, to apply it in tangent space, we could follow the steps below:**

- Calculate the initial tangent curve of the training rope, record the training trajectory
- Also record all tangent curve of the rope in each critical steps of training (we call it critical curve for convenience)
- Given a test rope, extract its tangent features
- Use an algorithm (TPS-RPM, CPD, etc.) to map the training tangent curve (in fact a bunch of discrete points on the curve) to the test curve. Record the transfer function $F$
- Use the same function $F$ to warp the initial training tangent curve, the resulted curve is $L$
- $L$ cannot be exactly the same as the test tangent curve, so find the closest point on the actual curve for each point on $L$, record their index in an array $Ind$
- For each critical step, for each point $i$ on rope, use the angle of point $Ind[i]$ on critical curve as the angle for the corresponding point on test rope. Thus we get the goal curve $L’$
- Use $L’$ to integrate and get the cartesian coordinates of the grasping points, the robot will then go to the appointed coordinate.

## Discussion

Despite the fancy result of TSM-RPM along with other algorithms such as tangent space CPD, problem remains as to their reliability. In the aforementioned method, we used the weighted sum of all different angles. In a straightening problem, for example, all zeros will of course sum up to zero, and result in a straight rope in whatever scenario. This is different from directly applying an algorithm in tangent space.

**Ideally**, once we can register the training tangent curve to the test tangent curve, we will be able to transform the tangent curve of every critical step from training to test. Once tangent curve is obtained, we can integrate out the cartesian position for grasping points, hence the entire trajectory. Seems that there is no need to calculate the correspondence between points. However, in practice we found that simply applying the algorithms in tangent space (instead of using the weighted sum method) will yield strange results.

Further analysis of these results show us that tangent space cannot be considered as a simple counterpart of cartesian space. Therefore, as long as we want to use these algorithms in tangent space, we will have to find out the correspondence of points and figure out the angles by weighted summation.

**Further discussions of this problem can be found in another post.**

## Reference

**The same reference list, unless specifically stated, will be used for all articles under this project here.**

See the introduction for reference.