I Introduction
Robotic learning from human demonstration is a powerful method for robotic arms to acquire a certain set of skills. Based on the idea that intelligent beings such as human acquire new skills by “imitation”, robots are programmed to imitate their trainers in a certain task [1]. For example, after one round of training by human, robots can learn to tie knots with different ropes and fold clothes of various sizes.
In previous works [2, 3, 4, 5], it is found that point set registration algorithms are promising in robotic learning from human demonstration. Point set registration is a process to construct a transformation function that maps one point set to another. Regarding its application in robotic learning, robots are first led by humans to manipulate a baseline object. The trajectory information during this training process is recorded by the robot. When faced with a similar object, the robot is able to do a quick mapping of the previous object to the current one by point set registration algorithm, obtaining a transformation function, and warp the recorded training trajectory with this function. The warped trajectory will be carried out to manipulate the new object. In this paper, the process of human demonstration is called “training”, and the process of manipulating a new object is called “testing”.
While point set registration most often finds its application in data alignment and information recovering, in this work it is employed to recognize object features and assign their correspondence accordingly. The method [4] has achieved success in many different experimental scenarios.
One unsolved problem is that the robot considers data collected by the camera as discrete points, and the warping function loses the object’s length feature. Take rope manipulation as an example. A rope is modeled as a point set which is uniformly distributed along the rope. Physics laws require that the distance between any adjacent points on a rope remains unchanged throughout the manipulation. However, a registered point set does not necessarily satisfy this condition. Therefore in some cases, the robot might overstretch the object, jeopardizing the manipulation. In previous works, we proposed a new algorithm
[6] to tackle this problem. A “tangent space” was constructed and point set registration was run in tangent space, after which the tangent information was integrated along the length of the rope, yielding its coordinates in Cartesian space. Simulation results showed that the algorithm successfully avoided overstretching and overcompression. In this work, a modified method is proposed using the same basic idea. The algorithm is verified in experimental settings with various applications. Experiments prove that our algorithm is able to generate a reasonable trajectory and carry out a manipulation whose result desirably resembles the training scene, while avoiding overstretching and overcompression. The algorithm works successfully both on ropes and cloths. This paper presents the modified algorithm, offering detailed implementation notes, analysis, and experimental validation.The remainder of this paper is organized as follows: Section II introduces related works of point set registration, as well as their novel application potential in robotic learning from human demonstration. Section III focuses on the coherent point drift (CPD) algorithm, and its application in robotic manipulation without considering the physical constraints of a soft body. Section IV elaborates on the proposed new algorithm, tangent space point set registration. Section V offers an insight into the experimental platform, with details of its system architecture. Section VI introduces experimental results and analysis.
Supplementary videos can be found at [7].
Ii Related Works
Point set registration is an algorithm developed to find a transformation function that registers one point set to another. Some of the wellknown algorithms include iterative closest point (ICP[8]), thin plate splinerobust point matching (TPSRPM[9]) and coherent point drift (CPD[10]). The ICP algorithm assigns a binary correspondence between points and finds a rigid transformation function. TPSRPM and CPD, on the other hand, apply soft correspondence with a probabilistic view, and can find nonrigid functions that register points better.
Though point set registration is not designed directly for robotic learning from demonstration, it has proven to be a powerful tool. In [4], the TPSRPM algorithm was utilized to teach robots to manipulate ropes. The point cloud of the training rope is mapped to the point cloud of the testing rope through TPSRPM, and the same function is used to warp the training trajectory, hence obtaining the trajectory in test. [11] and [12] extended this work by considering the constraints imposed by robot’s joints, obstacles in the space, and object’s surface normal. In [13]
, the CPD algorithm is proposed to track deformable object in real time by registering the state estimation to the measured point cloud. A uniform framework for state estimation, task planning and trajectory planning for deformable object manipulation was also presented in
[2] by utilizing CPD registration.It should be noted that most of the previous methods did not consider the geometric constraints of the manipulated object. Point set registration regards a physical object as a collection of discrete points and may therefore extend or shrink the object. We formerly proposed a method, TSMRPM[6] which tackles this problem in rope manipulation. TSMRPM fits a thin plate spline in tangent space, and integrates the sinusoidal and cosinoidal values of the rope angle along the rope length to get the Cartesian coordinates. TSMRPM proves effective in rope straightening and wire harnessing in simulation, but is not yet verified with experiments. In this work, the general point set registration algorithm is applied in tangent space to conserve the object length. As a specific implementation, the point set registration process makes use of coherent point drift algorithm[10] which, compared with previous algorithms, can be extended to higher dimensions and in general has a better registration performance under occlusion. Our work also for the first time realizes this idea in experiments, both with ropes and cloths.
Iii Manipulation by Coherent Point Drift
Iiia Coherent Point Drift Method
The coherent point drift(CPD) method was proposed to perform point set registration between similar point sets. The basic idea is to apply a probabilistic view in registration, and find out a “soft” instead of binary correspondence between points. Given two point sets, we divide them into observation set and reference set. The reference point set is transformed by function , resulting in a transformed reference set
. By maximizing the probability of the observation point set data being collected from the transformed reference point set, an optimal
is found.Given two point sets, the observation group and the reference group , the goal is to register to with function . , where is the position of the th node. . Let . To measure the precision of the transformation, calculate the probability that is an observation of
with a Gaussian mixture model(GMM). Note that the same weight is assigned for each point in
in GMM. For a GMM of uniform variance
, we have(1) 
To further deal with the problem of noise, an additional uniform distribution is introduced, with a uniform probability density of , where and stand for the total height and width of the area that is being observed (e.g. by the camera). Let . With this modification, we obtain
(2) 
where is some coefficient that is set manually, indicating the level of noise.
According to the maximum likelihood estimation, the goal is to maximize the total likelihood of being the observation result of . The total likelihood is defined as the multiplication of for each individual in . Let denote the size of . Take the negative natural logarithm of this likelihood multiplication, and the expression that needs to be minimized is obtained:
(3) 
where
(4) 
In the application of CPD, a regularization term is required that prevents the function from getting too arbitrary. This is important as the desired transformation should register adjacent points to roughly adjacent positions. This coherence is measured by the Fourier domain norm[14] of the function. Suppose is a transformation where . Its Fourierdomain norm is , where
is the Fourier transform of
and is a symmetric filter with as . The larger the Fourierdomain norm, the less smooth the function . Detailed mathematical explanation can be found in [14][15]. Applying this regularization, the expression of cost function that needs to be minimized is obtained(5) 
where
(6) 
In our specific implementation, in (6), is chosen as a Gaussian.
Also for , define an Operator such that
(7) 
This operator will be of use in future derivation steps.
Note that and are both unknown and need to be determined, while and are manually assigned.
Next, apply the classic EM algorithm[16] to find and .
Estep
The Estep uses previously achieved result to calculate
, which is a posterior probability describing the chance that
is an observation of or noise(when). Using Bayes’ theorem and the law of total probability, this probability can be calculated (the
superscript will be left out for convenience):(8) 
where and are as defined in (10) and (11) except that all probabilities are in step instead of step .
Mstep
The Mstep tries to minimize Q with respect to and . However, for the original expression (3) where is in the form of , no closedform maximization solution is available. In order to deal with this problem, apply Jensen’s inequality[17] such that an upper bound of is found as
(9) 
where
(10) 
(11) 
Instead of minimizing , which is mathematically difficult, we minimize . Further adding the regularization term, the final version of the function that calls for optimization is obtained
(12) 
Substitute (4) into (12) and ignore constants independent of and , and equation (12) can be simplified as
(13)  
where is a function of and .
It can be proved by calculus of variations that the function which minimizes is in the following form:
(14) 
where . Recall the operator defined in (7). is a Green’s function of , with being the adjoint operator to .
Also define so that everything can be written in matrix form. Now with this form, rewrite as a function of and , whereafter both and can be updated by setting ’s partial derivatives with respect to either of them to zero. The mathematical details can be found in[10].
Finally, function is updated as
(15) 
where is a kernel matrix with .
IiiB Application in Cartesian Space Trajectory Transformation
As CPD algorithm can register one point set to another with a function , it was proposed to do scenario warping in Cartesian space. In this section, this idea will be revisited.
Consider that in training and testing, two point sets of objects, and are given, respectively. Now use the algorithm described above to obtain a transformation function, , which warps , such that is close to the warped .
Note that during the training, the trajectory of the robot endeffector , was also recorded.
is a vector storing the coordinates of robot’s end effector at each critical time frame. Transforming
with yields . is taken as the new trajectory for robotic manipulation at test.This algorithm works well on tasks like rope knotting. However, it has serious problems when it comes to rope straightening. This is because point set registration methods see an object as a set of discrete points. Transformation does not preserve the distances between adjacent points, even though in practice this conservation is required. An example is illustrated in Fig. 2(b). Not meeting this physical requirement can result in failure or even damage of the object if it is to be stretched to its full length in manipulation.
Iv Trajectory Generation Algorithm
Iva Trajectory Generation in Tangent Space
In order to solve the problem of overstretching/overcompression, a new method of trajectory generation is proposed, which applies point set registration in tangent space.
First, the tangent information of a rope needs to be extracted. Let denote the angle between a node on the rope and the horizontal positive direction. A curve of as a function of node index can therefore be plotted. An example of this Cartesiantangent conversion is shown in Fig. 3. Note that the scale of axis can be modified arbitrarily without losing any essential information. Let us regard this plot in Fig. 3 as the tangent space representation of the rope. In a further step, combine all the into a vector and name it for training set and for test set.
Next, instead of applying point set registration in Cartesian space, calculate the warping function in tangent space. Performing on the new vector yields . Now calculate a correspondence matrix between and , so that the correspondence between each point pair in and can be obtained. As is considered as an observation of , the probability that is an observation of each can be calculated. This is a posterior probability, with the condition that is observed. With CPD algorithm, in the EM steps, this probability has already been derived (8).
Based on distances between point pairs, all the values in equation (8) can be obtained. Define a correspondence matrix . The value of reflects the degree to which is related to , hence to .
Next, determine the angle of each node on the target rope in test using a weighted average of the target rope during training. The desired tangent information for the test rope is
(16) 
In order to determine where the gripper should go during test, the first step is to find the grasping node on the training rope. is the node closest to the gripper in training. Let denote the grasping node for test, and it can be obtained through the following expression:
(17) 
Further integration yields the Cartesian coordinates of node .
(18) 
(19) 
where is the uniform length of each node on the rope, which is determined by the visual tracking system.
With this method, the length of the rope will always be conserved, since the gripper position is obtained by integration along the tangent direction of the rope.
Note that tangent space point set registration is not confined to coherent point drift method. As a general idea, any point set registration method can be applied. For example, the TPSRPM[9] algorithm will directly give us a correspondence matrix needed in (16), and we will be able to proceed from there. Different point set registration methods have their own advantages, here CPD is used as it can be generalized easily to any dimension and will enable us to do more complicated tasks in the future.
V Experiment Platform
Va Experiment Platform Architecture
The experiment platform consists of a dual robot system and a vision system, as illustrated in Fig. 5. An experiment consists of training and testing.
In training, the robots enter a “leadthrough teaching” mode where they can move according to forces applied to them. A human leads the robots through a trajectory, “teaches” them to operate, and records the information of several key frames, i.e. the position, attitude and status(open/closed) of two grippers. The vision system also records the state of the object before and after each operation.
A schematic illustration of the experiment platform is shown in Fig. 5. Two FANUC LR Mate 200iD/7L robots are used to manipulate the object. Two Kinect Model 1517 cameras are placed in different directions to collect visual data, transmitting 640480 RGBD image at 10Hz to Ubuntu PC, which runs a ROS[18] node. Our vision algorithm[13] abstracts the object as a set of nodes, and returns an array of Cartesian coordinates of all nodes at a slightly lower rate depending on the size of the object. The processed information, in the form of published ROS topic, is sent to Host PC, which runs the algorithm discussed above and generates the trajectory. Finally, the trajectory is sent to target PC, where the code that controls the robots is carried out.
VB Parameters
For coherent point drift algorithm, some parameters are tuned in order to achieve a balance between rigidity and precision. First, in order for the curves to be mapped together well, the scale of all points in and directions should be similar. In our experiment, the value is chosen to be the node index multiplied by , while value is chosen to be the angle value in degrees.
The mapping function cannot be too arbitrary or it will directly map each point in training to testing evenly along the length, losing track of features such as corners. In experiment, the regularization coefficient in (5
) is chosen to be 10. Since outliers are already filtered out, the term for outliers,
, is set to be 0. The standard deviation of the Gaussian distribution for Green’s function G in (
14), here denoted by , is set to be 1. To save time without sacrificing the mapping precision, the maximum iteration for point set registration EM step is set to be 100.Vi Experiment Results
Despite the fact that tangent space point set registration was first proposed to solve problems occurred in rope straightening, the algorithm is found to perform well on various tasks, such as cloth folding, cloth unfolding and knot tying. Two examples are shown in this section to offer a better understanding of its capability.
Via Cloth Unfolding
In this section, the performance of the proposed algorithm (point set registration in tangent space) with the previous method (point set registration in Cartesian space) will be compared for the task of cloth unfolding.
For this experiment (Fig. 6), in training, the cloth is 35cm in direction and 46cm in direction. In test, the cloth is rotated by 90 degrees, therefore it has a dimension of 46cm in and 35cm in . Furthermore, the folding angles are also different.
In Fig. 6 (a1)(a2), the manipulation of unfolding the cloth is demonstrated to the robot by leadthrough teaching. For cloth unfolding, we are interested in the upper edge of the cloth, which is marked in yellow. As is shown in Fig. 6(b1)(b2), tangent space point set registration is able to unfold the cloth without overstretching or overcompressing. Note that the algorithm first extracts the upper edge of the cloth and runs on the upper edge.
On the other hand, Cartesian space point set registration fails to accomplish this task. In Fig. 7(c), the robots were faced with a narrow, folded cloth. In an effort to unfold it, they overcompress the cloth. Measurement shows that the upper edge shrinks in direction by 4cm, verifying our prediction in Fig. 2. Furthermore, it has a 5cm shift in direction due to the warp of space. In experiment, these deviations resulted in a very wrinkled cloth, as shown in Fig. 7(c). Wider cloths, on the other hand, will be overstretched, as shown in Fig7(b).
ViB Rope Knotting
Using point set registration in tangent space will yield a trajectory that can conserve the object’s length, hence safer manipulation. So far, it has been shown that for cloth unfolding, point set registration in Cartesian space will not work. This also applies to any other manipulation that involves stretching an object to its full length.
In other scenarios, where formerly point mapping in Cartesian space succeeded, the proposed algorithm is also robust and safe. Rope knotting is the typical task the previous method was able to master (because the rope is unlikely to be fully stretched in a knotting task). Tangent space point mapping also works on rope knotting. A typical rope knotting task is divided into 4 steps, with 5 different rope states. A snapshot of each of these rope states is taken and tangent space point set registration is performed for every step.
As an example, Fig. 9 demonstrates how the algorithm works in step 2.
Step by step, the robotic arms are able to carry out the entire task of knotting.
Vii Conclusion and Discussion
In this work, point set registration in tangent space is proposed to transfer a robot’s skill learned in training to test scenarios. Compared with the previous method which runs in Cartesian space, point set registration in tangent space is able to conserve the length of the manipulated object and satisfy physical constraints.
When it comes to 2D object (such as cloth) manipulation, cases where the object is stretched becomes more common due to the increase in physical constraints. Therefore, the successful experiment results of tangent space point mapping indicate more possibilities for robots to operate on 2D objects.
References
 [1] B. D. Argall, S. Chernova, M. Veloso, and B. Browning, “A survey of robot learning from demonstration,” Robotics and Autonomous Systems, vol. 57, no. 5, pp. 469 – 483, 2009.
 [2] T. Tang, C. Wang, and M. Tomizuka, “A framework for manipulating deformable linear objects by coherent point drift,” IEEE Robotics and Automation Letters, vol. 3, no. 4, pp. 3426–3433, 2018.
 [3] T. Tang and M. Tomizuka, “Track deformable objects from point clouds with structure preserved registration,” The International Journal of Robotics Research, p. in review, 2018.
 [4] J. Schulman, J. Ho, C. Lee, and P. Abbeel, Learning from Demonstrations Through the Use of Nonrigid Registration. Cham: Springer International Publishing, 2016, pp. 339–354.
 [5] H.C. Lin*, T. Tang*, Y. Fan, and M. Tomizuka, “A framework for robot grasping transferring with nonrigid transformation,” in Intelligent Robots and Systems (IROS), 2018 IEEE/RSJ International Conference on. IEEE, in review.
 [6] T. Tang, C. Liu, W. Chen, and M. Tomizuka, “Robotic manipulation of deformable objects by tangent space mapping and nonrigid registration,” in 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Oct 2016, pp. 2689–2696.
 [7] Experimental video for tracking deformable objects in real time, http://www.aray.pub/paper/pointsetregistrationintangentspace.html.
 [8] P. J. Besl and N. D. McKay, “Method for registration of 3d shapes,” in RoboticsDL tentative. International Society for Optics and Photonics, 1992, pp. 586–606.
 [9] H. Chui and A. Rangarajan, “A feature registration framework using mixture models,” in Mathematical Methods in Biomedical Image Analysis, 2000. Proceedings. IEEE Workshop on. IEEE, 2000, pp. 190–197.
 [10] A. Myronenko and X. Song, “Point set registration: Coherent point drift,” Pattern Analysis and Machine Intelligence, IEEE Transactions on, vol. 32, no. 12, pp. 2262–2275, 2010.
 [11] A. X. Lee, S. H. Huang, D. HadfieldMenell, E. Tzeng, and P. Abbeel, “Unifying scene registration and trajectory optimization for learning from demonstrations with application to manipulation of deformable objects,” in Intelligent Robots and Systems (IROS 2014), 2014 IEEE/RSJ International Conference on. IEEE, 2014, pp. 4402–4407.
 [12] A. X. Lee, A. Gupta, H. Lu, S. Levine, and P. Abbeel, “Learning from multiple demonstrations using trajectoryaware nonrigid registration with applications to deformable object manipulation,” in Intelligent Robots and Systems (IROS), 2015 IEEE/RSJ International Conference on. IEEE, 2015, pp. 5265–5272.
 [13] T. Tang, Y. Fan, H.C. Lin, and M. Tomizuka, “State estimation for deformable objects by point registration and dynamic simulation,” in Intelligent Robots and Systems (IROS), 2017 IEEE/RSJ International Conference on. IEEE, 2017, pp. 2427–2433.

[14]
F. Girosi, M. Jones, and T. Poggio, “Regularization theory and neural networks architectures,”
Neural Computation, vol. 7, pp. 219–269, 1995.  [15] Z. Chen and S. Haykin, “On different facets of regularization theory,” Neural Computation, vol. 14, no. 12, pp. 2791–2846, 2002.
 [16] A. P. Dempster, N. M. Laird, and D. B. Rubin, “Maximum likelihood from incomplete data via the em algorithm,” Journal of the Royal Statistical Society. Series B (Methodological), vol. 39, no. 1, pp. 1–38, 1977.
 [17] M. Kuczma, An introduction to the theory of functional equations and inequalities: Cauchy’s equation and Jensen’s inequality. Springer Science & Business Media, 2009.
 [18] The Robot Operating System (ROS), http://www.ros.org/.
Comments
There are no comments yet.