Trajectory reconstruction for robot programming by demonstration

ABSTRACT


INTRODUCTION
Since the invention of the first robots, the reproduction of human movement is still a challenging subject in robotics. This reproduction can be divided into two categories: the first one is imitation of the human movement as it is by the robot to realize the task. References [1,2] they imitate the human movement to realize different tasks like writing and opening doors, their approach was derived from the human functioning based on the body schema and the percept. In the work of Jie and al. [3], they focus on the pose imitation between a human and a humanoid robot and proposed a pose similarity metric based on the shared structure of the motion spaces of human and robot .The second reproduces the human movement by taking as reference only the hand (the end-effector) and neglecting how and where the other joints are positioned (shoulder, elbow, and wrist). In the work of Benchikh [4], in which he realized a system of a synchronous reproduction of the human movement, following the movement of a single point of interest.
One can notice two categories of robot programming methods, the first one is manual like the text based systems, graphical systems and the teach-pendant programming; the second one is the automatic programming such as the instruction and the observation based programming. The Figure 1 represents the categorization of these methods made by [5]. For applications such as imitation and reproduction of  [1-4, 6, 7].

Figure 1. Robot programming methods
Programming by Demonstration (PbD) is a robot programming method based on the extraction of data directly from the visualization of the user's performance. It is a promising automatic method, which can permit to a user with little or no expertise to program robot tasks [8,9]. In Pbd systems, the teacher performs the task while a learning interface records the movement and actions carried out during the performance. Different interfaces can be used. Kinesthetic guidance teacher moves the robot links manually and the trajectories are recorded [7]. The direct control and teleoperation interfaces are also used in Pbd systems [10]. Interfaces based on sensors (vision, magnetic and inertia) are widely used in Pbd systems. There is a variety of tracking techniques in vision systems [11][12][13][14] and it has an advantage over the other sensor-based methods, because no sensors need to be attached to the teacher. And the vision systems allow the teacher to have more natural performance without being disturbed by the material.The sequence measurements data usually goes through a scaling step through interpollations. For the time series data and from different types of interpolation techniques, polynomial interpolations are the mostly used. The cubic spline interpolation was used to reconstruct the Cartesian trajectories in [15] and Non-Uniform Rational B-Splines (NURBS) for the trajectory approximation [16]. The L1 splines for the interpolation and preservation of the trajectory [17][18][19].
In this paper, we propose a Pbd system in trajectory level based on visual tracking system (ArToolKit) to track the hand/tool movement. In this work we used the cubic spline interpolation to reconstruct the Cartesian trajectories. Compared to previous works; the constrained cubic spline interpolation is simple to implement and efficient. Finaly the caputed movements are reproduced by a manipulator robot in a simulated environment.

THE TRACKING SYSTEM
For the hand/Tool tracking, we propose to use a vision-based tracking system. These systems use image-processing methods to calculate the camera pose relative to real world objects and give the position and orientation of these objects. In our work, we use the ARToolKit for tracking the user's hand/tool. We fixed the markers on the faces of a cube (the same marker for all faces). This will allow the camera to see at least one marker and permit us to avoid occlusions. Based on the works [20,21] we have used simple markers with a 30% border width to have a better detection of the marker and avoid at the maximum the false identifications. In the Figure 2, we can see an example of the markers that are using. The operator will perform the movements and the system will track the position of the marker for the hand/tool and gives the position (x, y, z) every sampling time (the position is calculated from the centre of the cube). The acquisition camera was set at 20 frames/s to limit marker miss identifications. The slow frame rate and some miss identifications created gapes and therefore, an interpolation of the acquired tracking data was mandatory.

THE CONSTRAINED CUBIC SPLINE
The principle behind constrained cubic spline is to prevent overshooting and eliminating oscillation by sacrificing smoothness. In this interpolation, we replace the equality of the second order derivatives at every point by a specified first order derivatives [22,23]. The construction of the constrained cubic spline function is based on the following criteria: -Curves are third order polynomials -Curves pass through all the known points -First order derivative, is the same for both functions on each side of a point.
-Boundary conditions are the same as for the natural cubic spline. 1 -The second order derivative is replaced by a specified first order derivative at every point.
The main step becomes the calculation of the slope for each point. Naturally, we know the slope will be between the slopes of the adjacent straight lines, and it should approach zero if the slope of either line approaches zero.
In this interpolation, there is no necessity to solve a system of equation because the slope at each point is known. Based on the two adjacent points on each side. We can calculate every spline function; as given by (1) by using (8) to (13).
Finally, every polynomial is calculated from the following parameters:

THE ROBOT'S MODEL OF MOTION
In our work, we use a manipulator robot model, the Puma 600. The parameters of this robot are shown in Table 1 [24,25]. Motion model of such a mechanism is usually described by the following matrix equation: where: Γ : Vector of actuator joint torque M(q) : Inertia matrix C(q, q̇)q̇ : Vector of centrifugal and Coriolis torque G(q) : Vector of gravitational torques F(q̇) : Vector of actuator joint friction forces q, q̇, q̈ : Are respectively, the joint angle, velocity, and acceleration vectors To ensure the linearization of the nonlinear system described by (14) in closed loop, we introduce a linearization control system based on exacting knowledge of the robot model and its implementation. In this control system, the loop of the linearization is achieved by choosing a torque Γ applied to the robot, as follow: Γ 0 is an auxiliary input of the select controller. A proportional derivative control (PD) is a typical choice and it is given by the equation:

SIMULATION AND RESULTS
In this section, Since the trajectories obtained from the tracking system should be interpolated, we started by testing the constrained cubic spline interpolation on different data sets. The Figures 4 and 5 show a comparison between the constrained cubic in blue line and the conventional cubic spline interpolation in red line, the red circles are the interpolated data points. In the first tests, we used both of the interpolation methods to reconstruct handwriting trajectory made at almost the same speed. We have chosen to reconstruct the letter M using both of the cubic spine interpolation and the constrained cubic spline because this letter contains sudden directional changes which will help us to see the behaviour of the used interpolation method. In the Figure 4 1,2,3,4,5,6,7,8,9,10] ; y=[0,0,0,0,0,1,1,1,1,1,1] (b): x=[0, 1,2,3,4,5,6,7,8,9,10,11,12,13,14] ; y=[0,0,0,0,0,1,1,0,0,0,-1,0,0,0,0] In Figure 5, it is noted that the constrained cubic spline does not oscillate after large amplitude variation and reduces the overshooting comparing to the cubic spline. This fact makes the constrained cubic spline more stable and has less oscillations which makes it better for the reconstruction of the trajectories. The interpolation programs were made with Matlab 2017a on a computer with the following configuration: I7 3770 3.4 Ghz and 8 Gb Ram. Table 2 shows interpolation execution time according to interpolated point number. One can notice two behaviours. The first is for number of points below 200: the constrained cubic spline is faster than the cubic spline since it does not solve a system of equations. The second case is when the number of interpolated points exceeds 200 the cubic spline overcomes the constrained cubic spline. In the Figure 6 one can see a handwriting trajectory reconstructed with the constrained cubic spline interpolation. Figure 7 shows a simulation of Puma 600 robot executing the same trajectory. The advantage of the simulation process is reconstructed path visualization. The proposed system depends on the teacher, and do not generate collision-free trajectories.   Figure 6. 3D Trajectory reconstructed by the constrained cubic spline Figure 7. Simulation of robot trajectory reproduction

CONCLUSION
This paper presented a robot programming by demonstration system in a trajectory level. The system is based on human movement reproduction by following a single point of interest (the hand/tool). We used ARToolkit as a visual tracking system. The slow frame rate and the miss identifications of the markers was creating gapes in the trajectories. The natural cubic spline and constrained cubic spline interpolations were used to correct these weaknesses and to reconstruct the trajectory. A comparison was made between those both interpolation methods for 3D trajectories reconstruction, 1D data sets and as per execution time. Tests show that constrained cubic pline interpolation overcomes the cubic spline interpolation. The constrained cubic spline interpolation shows good results and the fact that it generates less oscillation and prevents overshooting makes it suitable for the trajectory reconstruction. In the future works this it will be used in a real time trajectory reconstruction.