A Smooth Orientation Planner for Trajectories in the Cartesian Space

Trajectories in the operational space, when conceived for manipulators with more then three degrees of freedom, impose the adoption of an orientation primitive for the end-effector. The planning complexity increases if smoothness represents one of the motion requirements and the trajectory is obtained through the combination of several basic primitives. In this eventuality, vibrations and mechanical solicitations can be reduced by avoiding motion stops at the end of each segment. Good tracking performances can be conversely achieved by guaranteeing jerk-continuous reference signals for the actuators. The orientation planner proposed in this letter allows the smart generation of smooth trajectories. As experimentally proved in the work, the novel planning primitive is characterized by very short computational times.


A Smooth Orientation Planner for Trajectories in the Cartesian Space Andrea Tagliavini and Corrado Guarino Lo Bianco , Senior Member, IEEE
Abstract-Trajectories in the operational space, when conceived for manipulators with more then three degrees of freedom, impose the adoption of an orientation primitive for the end-effector. The planning complexity increases if smoothness represents one of the motion requirements and the trajectory is obtained through the combination of several basic primitives. In this eventuality, vibrations and mechanical solicitations can be reduced by avoiding motion stops at the end of each segment. Good tracking performances can be conversely achieved by guaranteeing jerk-continuous reference signals for the actuators. The orientation planner proposed in this letter allows the smart generation of smooth trajectories. As experimentally proved in the work, the novel planning primitive is characterized by very short computational times.
Index Terms-Motion and path planning, task and motion planning, orientation planner, jerk continuity.

I. INTRODUCTION
T HE orientation planning problem is common to many automation contexts and can be summarized as follows: given a sequence of through points, a trajectory must be planned so that the end-effector could exactly cross all of them with the desired orientation. Typically, generated motions should simultaneously satisfy some additional features, and smoothness is certainly one of them. To this purpose, reduced solicitation can be achieved by guaranteeing that the actuators reference signals are jerk-continuous.
Orientations can be expressed in multiple ways and, consequently, different planning techniques can be conceived. The most commonly used notations were compared in [1] in terms of effectiveness and conciseness. Rotation matrices have an immediate physical meaning but, being based on 9 terms, are certainly redundant. Consequently, they are not immediately suited to planning purposes. Euler angles return a compact orientation description, but they lack of effectiveness in some particular Manuscript  configurations. Angle-axis representation and quaternions are the most effective orientation notations: they both use four terms (one of them is clearly redundant) and are not subject to singular configurations.
One of the first planning strategies specifically conceived for quaternions was proposed by Shoemake in 1985 [2]: the Spherical Linear Interpolation (SLERP). Such strategy is well suited for the management of point-to-point trajectories, but it is not immediately usable in multi-point contexts, which, conversely, are very common in robotics applications. When the motion involves multiple via-points, the SLERP approach causes "orientation corners," which induce discontinuities in the actuators speeds, accelerations and jerks. The "orientation corner" problem is equivalent to the well known "path corner" problem arising in CNC machines: in both cases discontinuities can be avoided by admitting trajectory approximations. In order clarify the concept, let us consider the path generation problem for a CNC machine schematically shown in Fig. 1 (the orientation planning problem poses similar problems). Such figure shows a situation in which a generic curve, represented by the black solid line, should be followed by a milling tool. As known, in CNC applications the original path is preliminarily converted into a sequence of via-points: at run-time the machining tool executes a series of linear segments joining such points (see the blue dotted lines), thus introducing a first approximation with respect to the desired profile. In order to eliminate discontinuity issues caused by the adoption of straight line primitives, corner smoothing methods (see the green dash-dotted lines) have been proposed [3], [4], [5], [6]. However, such strategies, as shown in Fig. 1, induce further approximations w.r.t the nominal path.
An equivalent problem arises for orientations trajectories when using the above mentioned SLERP primitive. In order to partially overcome the continuity issues, an alternative primitive, named SQUAD, was proposed in [7]. However, as shown in [8], such orientation primitive only guarantees the C 1 continuity of the reference signals at the via-points. Previous considerations make it possible to assert that, for an accurate generation of the surface profile, particular care must be dedicated to the This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/ generation of both the path and the orientation trajectories. More in details, path approximations can be strongly reduced, for example, by adopting a primitive recently proposed in [9]: a curve which exactly crosses the assigned via-points can be generated by interpolating them through the η 3D -splines. As indicatively shown by the yellow dashed lines in Fig. 1, such primitive allows path errors which are smaller than the ones that can be achieved with the corner smoothing strategy. However, in order to implement a fully working planner in the operational space, such path primitive needs to be combined to an orientation planner. This letter is devoted to the design of such planner, so as to associate a proper orientation to each point of the η 3D -splines path. The proposed primitive is characterized by a C 3 continuity level.

A. State of the Art
As previously stated, continuity problems can be overcome by means of orientation corner smoothing techniques, like the ones proposed in [3], [4], [5], [6]. Alternatively, new primitives have been designed for the direct smooth interpolation of the quaternions assigned at the via-points. To this purpose, the literature proposes several approaches, which differ each other for the adopted primitive: to obtain the C 1 continuity Shoemake [7] and Dam et al. [8] use a spherical interpolation, while to obtain the C 2 continuity Legnani et al. [10] use a repeated combination of SLERP curves, Kim et al. [11], Ge et al. [12] and Pu et al. [13] use B-splines, Nielson [14] uses ν-splines, Liu et al. [15] use quartic polynomials while Tan et al. [16] use quintic polynomials. It is important to point out that all the aforementioned strategies [7], [8], [9], [10], [11], [12], [13], [14], [15], [16] only guarantee the C 1 or the C 2 continuity of the orientation profiles. Consequently, the corresponding actuators jerks are discontinuous. The sole alternative strategies proposed in the literature, which generates position and orientation trajectories in the operational space and also guarantees jerk-continuous signals, are the ones proposed in [5], [6].

B. The Novel Planner
In this letter, in order to avoid jerk discontinuities and to obtain very smooth trajectories, a novel orientation planning approach, able to guarantee the C 3 continuity of the reference signals, is designed. The new primitive is specifically conceived to be used together with the η 3D -splines [9] path planner. When used together, the two primitives allow the generation of jerk-continuous trajectories. A common denominator of both primitives is represented by their low computational burden. The orientation planner proposed in this work could also be combined with alternative G 3 path primitives, like the one given in [17] or, if jerk continuity is not mandatory, with G 2 primitives [18], [19].
The novel planner differs from the ones proposed in [5], [6] for a relevant aspect. While all the three approaches guarantee the C 3 continuity -actually [5] allows the C 4 continuity -the planners designed in [5], [6] are explicitly conceived to handle corner smoothing problems, so that trajectories are made of linear segments properly joined at the via-points by means of blending curves. As seen earlier, this implies that the assigned positions and orientations at the via-points are satisfied with a given tolerance. Conversely, the trajectory planner proposed in this letter allows a strict fulfillment of the assigned positions and orientations. This is possible because the η 3D -splines can generate a wide variety of curves -like, for example, linear segments, circular arcs, clothoids, conic spirals, etc. -directly passing through the assigned via-points (see [9] for more details), so that corner smoothing techniques, which necessarily cause tolerances, are not required at all.
The letter is organized as follows. Section II introduces some preliminary considerations on the continuity characteristics which must be owned by the orientation planner in order to achieve continuous jerk signals. In the same section, some basic concepts on the quaternion theory are recalled. Section III proposes the orientation planner. Tests on the new primitive are proposed in Section IV. First, a simulated case is considered for a CNC milling problem, in order to prove that the orientation planner can be used in multiple contexts. Then, experimental tests have been executed with a Comau Smart SiX 6.14 manipulator and the corresponding outcomes are discussed in the same section and are visually shown by means of two multimedia attachments. Final conclusions are drawn in Section V.

II. PRELIMINARY CONSIDERATIONS ON THE GENERATION OF JERK CONTINUOUS SIGNALS
The main purpose of this work is to devise a new orientation planning primitive for trajectories in the operational space, so as to guarantee very smooth movements of the actuators. More precisely, the generated Cartesian trajectories must guarantee that the third order derivatives of the corresponding joint trajectories, i.e., ... q , are continuous. Given a manipulator with n degrees of freedom, a direct relationship exists between a point in the configuration space, i.e., q ∈ R n , and the corresponding point in the operational space. In this work, points in the operational space are represented through a position p ∈ R 3 and an orientation ε ∈ R 4 , the latter expressed through the unit quaternion representation. As a consequence, the continuity on q implies, in turn, the continuity on both p and ε.
Analogously, velocities in the two spaces are correlated through the Jacobian matrix J(q), according to the following expression where is the generalized velocity vector, which contains the linear and the angular velocities of the end-effector. The applications considered in this work involve non-redundant systems, so that their Jacobian matrices, far from singularities, are invertible. Consequently, (1) can be rewritten as followsq Equation (2) makes it possible to assert that the continuity ofq is achieved by guaranteeing the continuity on v N . Similar considerations hold for the higher order derivatives. J(q) is continuously differentiable, so that some algebraic manipulations on the derivatives of (1) w.r.t. the time, lead to the following expressions T are the first and second time derivatives of v N which, necessarily, need to be continuous in order to guarantee the continuity ofq and of ... q . The aforementioned considerations indicate that C 3 trajectories in the joint space can be achieved by means of trajectories in the Cartesian space which fulfill some specific path and orientation properties. A complete analysis on the path characteristics can be found in [9] and it is here omitted for conciseness. Conversely, in this letter the attention is focused on the orientation planning problem: specific conditions for the generation of smooth profiles are provided in the following.
A direct relationship between ω andε can be found according to the following reasoning. The end-effector orientation can be represented through a rotation matrix R or, alternatively, through ε. The two representations are each other correlated by the following expression R = r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 As known, the following relation applies S(ω) =Ṙ R T = ṙ 11ṙ12ṙ13 r 21ṙ22ṙ23 r 31ṙ32ṙ33 r 11 r 21 r 31 r 12 r 22 r 32 r 13 r 23 r 33 (4) where is a skew-symmetric matrix which depends on the angular velocity of the end-effector ω : The components of ω can be extracted from (4). In particular, a few algebraic manipulations make it possible to write By substituting the terms of (3) into (5), after a few algebraic manipulations the following relationship betweenε and ω is finally obtained where The inverse relationship between ω andε can be obtained according to the following reasoning. Let us add an additional row to (7), so as to obtain the following matrix It can be easily proved that ε andε are each other orthogonal, so that ε Tε = 0. Consequently, (6) can be rewritten as follows Matrix M * (ε) is non-singular since it can be verified that Indeed, the following condition applies As a consequence, (8) can be inverted as followṡ or, more compactly, as followṡ where M(ε) is given by (7). Due to (6) and (9), it can be asserted that the continuity of ω is achieved through the continuity ofε.
By differentiating (6) the following equations are further obtained i.e., the continuity on α and ι is mapped into equivalent continuity conditions onε and ... ε .

III. THE C 3 CONTINUOUS ORIENTATION PLANNER
According to the discussion in Section II, the C 3 continuity of the trajectory in the configuration space imposes, in turn, that also the planning primitive adopted for ε must be C 3 . In order to guarantee a strict relationship between orientation and curvilinear coordinate, ε is planned as a function of the curvilinear coordinate s. Consequently, a trajectory for ε is obtained by combining ε(s) with a timing law s(t). The time derivatives of the orientation trajectory can be expressed as followṡ where ε := (dε)/(ds), ε := (d 2 ε)/(ds 2 ), and ε := (d 3 ε)/(ds 3 ). Clearly, the required C 3 continuity on ... ε is achieved if ε(s) and s(t) are both C 3 continuous. For conciseness, the dependency of ε on s is dropped in the following.
The orientation primitive could be potentially planned by only considering components ε 1 , ε 2 , and ε 3 . The fourth component, i.e. ε 0 could be subsequently obtained by imposing condition ε = 1. However, such planning strategy would lead to complex expressions for ε , ε , and ε . For such reason, the four components of the quaternion are independently planned. The resulting vector, i.e.,ε, is later normalized in order to fulfill condition ε = 1 by means of the following equation The derivatives of ε w.r.t. s can be easily obtained through some algebraic manipulations. More precisely, it can be easily proved that they can be represented as follows ε =ε ε + 3αε + 3βε + γε, where α = d ds Under the hypothesis that ε = 0, and since (10)-(15) only contains sums, products, and ratios of continuous functions, according to [20] (see p. 59) one can assert that continuity properties ofε are inherited by ε, i.e., ifε is C 3 , then ε is C 3 as well.
According to the premises, the components ofε are planned independently. Thus, a scalar primitive is adopted for each of them: in the reminder of this section it is indicated as f (s).
Given a set of n + 1 via-points points, whose displacement along the curve is given through the corresponding curvilinear coordinates s = {0, s 1 , . . . , s n }, function f (s) can be partitioned into a set of n sub-functions defined as follows By definingŝ := s − s k−1 , functions f k (·) in (16) can be rewritten as follows Many alternative primitives can be adopted for f k (ŝ), k = 1, 2, . . . , n. A nice survey is proposed in [21] for the achievement of composite C 2 function. In this letter, a possible strategy is devised in order to achieve the required C 3 continuity. Function f k (ŝ) is defined as follows The polynomials coefficients can be evaluated by solving a linear system which can be set up by considering a proper set of interpolating conditions. For example, since the orientations at the via-points are assigned,f 0 := f 1 (0),f k := f k (ŝ k ), k = 1, 2, . . . , n are known and must be satisfied by the polynomials.
In the strategy next proposed, jerks at the beginning and at the end of each segment of the composite trajectory are posed equal to zero in order to limit the signal variability. Furthermore, two additional free-displacement points are added in the middle of the first and of the last intervals of the composite function (see also [21]) in order to balance the number of equations and unknowns. This implies thatn = n + 2 polynomial functions must be generated.
The following conditions must be satisfied in order to generate a C 3 composite function r imposition of the initial and final values for each segment of the composite curve (2n − 4 conditions) It is worth noticing that the positions of the freedisplacement points have not been assigned; r imposition of the composite function continuity at the freedisplacement points (2 conditions) r imposition of the speed and the acceleration continuity Summarizing, the planning problem requires solving the following linear system made of 6n equations in 6n unknowns where x : = [a 01 a 11 a 21 a 31 a 02 a 12 . . . ann] T ∈ R 6n is the vector of the unknowns, A ∈ R 6n×6n , and b ∈ R 6n . Some algebraic manipulations make it possible to reduce the order of (17), so that A is converted into the following three-diagonal matrix with A ∈ R (n−1)×(n−1) . Analogously, the structure of b ∈ Rn −1 changes as follows and x = [a 22 a 23 . . . a 2n ] T ∈ Rn −1 becomes the vector of the unknowns. Efficient algorithms exist for the inversion of three-diagonal matrices [22], so that large values ofn can be easily handled. Once the reduced order system has been solved, the remaining coefficients of the polynomials can be obtained through the following closed form equations a 0n = 6a 2n + 7f n 20ŝ 2 n +f nŝn +fn, a 3j = 0, j = 1, 2, . . . ,n, The aforementioned solution applies forn > 4, i.e., when the problem admits more than 3 planning points. Ifn = 4, matrices A and b of (17) change as follows The remaining coefficients are still obtained through (18)-(34) by assumingn = 4. Remark 1: As previously stated, condition ε = 0 must be strictly satisfied so as to guarantee that normalized ε is C 3 . Possible violations could be caused by the independent planning of the 4 components ofε. However, in applications like the ones considered in this work, adjacent via-points are each other very close and their orientation changes are minimal: along each segment of the composite path the components ofε change minimally and almost linearly, so that ε 1 along the whole trajectory (see also Fig. 7 in Section IV).

IV. SIMULATIONS AND EXPERIMENTAL RESULTS
In order to obtain a C 3 timing law, a jerk-continuous s(t) profile is needed. Typically, Cartesian trajectories are used for applications which require a constant speed of the tool frame  (machining, gluing, soldering, laser cutting, etc.). For this reason, a simple timing law, like the one shown in Fig. 2, was adopted for the experiments. More complex shapes can be assumed for alternative tasks, provided that the resulting function is C 3 : for example, in order to satisfy a set of given velocity or acceleration limits, a trajectory scaling strategy like the one proposed in [23] can be used. The timing law planning problem is not considered in this letter, so that no further details are given.
The orientation planner was tested by means of simulations concerning a CNC application and by executing a real trajectory with a Comau Smart SiX 6.14 manipulator, a six link anthropomorphic robot. More precisely, the simulated test concerned a sphere milling problem and was conceived so as to prove the effectiveness of the planner for machining purposes. The experiment on the real manipulator consisted in the generation and the execution of two composite trajectories. The end-effector path was generated by means of the η 3D -splines planner proposed in [9], while the orientation was provided through the primitive proposed in this letter.
In the simulated test case, a milling trajectory was planned for a spherical surface. Fig. 3 shows its shape. When finishing an object, the perpendicularity between the milling cutter and the surface -or the preservation of a given angle between them -is critically important in order to guarantee both efficiency (in terms of chip rate, bit temperature, etc.) and surface quality. The proposed example assumes that the tool tip axis is always aligned with the radius of the sphere, so that the planner effectiveness is checked by considering the error angle between the axis of the milling tool and the normal vector of the surface. It was verified that the largest errors occur when the path direction suddenly changes: at the turning points, the milling precision can be improved by increasing the density of via-points, as shown by the detail in Fig. 3. A maximum error equal to 6.5 · 10 −4 rad was  achieved along the whole path by assuming the via-points shown in Fig. 3: apart from the changes of direction, a very limited number of via-points are required to mill the entire surface.
As noted earlier, the second set of experiments involved a real industrial manipulator. Two different trajectories were considered. As shown in Fig. 4, the first trajectory is composed by a circular arc immediately followed by a straight segment. The same figure also shows the assigned orientations of the endeffector frame at the via-points. The two paths are joined through a Cartesian curve still obtained by means of an η 3D -spline curve. The whole path is G 3 . The jerk reference signals for the six joints, obtained by means of the proposed planner, are shown in Fig. 5: as expected, they are continuous.
The second manipulator experiment exploits a composite path made of circular arcs, linear segments, conic spirals, and helical curves generated by means of the η 3D -splines. As shown in Fig. 6, proper orientations have been assigned in each via-point of the path. In order to show the flexibility of the proposed planner, a constant orientation has been assumed for the first Fig. 6. The trajectory adopted for the second experiment on the manipulator. Black dots indicate the assigned via-points, while the reference frames highlight the desired end-effector orientations. The end-effector longitudinal axis is aligned to theẑ unit vector. segments of the composite trajectory, while the tool orientation changes during the execution of the conic spirals and of the helical curves. More precisely, the tool-frame rotates around its ownŷ axis, so that itsx andẑ axes remain confined inside a vertical plane. Fig. 7 shows that, as expected, ε is everywhere very close to 1, even considering the final peak which occurs in a segment whose via-points are very far each other. Consequently, as shown in Fig. 8, ε is a continuous function and, it turn, joint jerks are continuous as well (see Fig. 9).
The acquired orientation errors, i.e. the angular differences between the unit vectors of the trajectory frame and the ones of the tool frame, these latter obtained from the encoders readings, are shown in Fig. 10. Thanks to the jerk continuity (the corresponding figure has been omitted for conciseness), the absolute values of the errors do not show evident peaks along the whole path but, conversely, they remain confined within a constant strip whose amplitude is equal to 4.2 · 10 −3 rad.
The orientation planner was conceived to be used in rapidly changing scenarios. Consequently, its computing times have been checked in order to quantify the computational burden. The planning algorithm was run by considering 998 test sets. More in detail, the nth set, with n = 3, 4, . . . , 1000, was made of n randomly chosen via-points. Tests were executed on one single   core of an Intel i7-1165G7 processor running at @2.80 GHz. The investigation made it possible to establish that the computational burden depends linearly on the number of via points. More precisely, the computational time is equal to n × 2.19 · 10 −7 s, so that the trajectory shown in Fig. 4, which is composed by 127 segments, is planned in 2.781 · 10 −5 s. Consequently, a trajectory made of 4500 points could be processed within 1 ms.
The attached multimedia files visually show the execution of the manipulator trajectories. A constant longitudinal speed equal to 0.15 ms −1 was assumed for the first experiment, while it was raised to 0.3 ms −1 for the second one. As shown by both videos, trajectories are fluently executed and no stops are required at the via-points.

V. CONCLUSION
The orientation planner proposed in this work makes it possible to generate, in a straightforward way, composite trajectories in the operational space. Its combination with the η 3D -splines, a recently devised path planning primitive, guarantees that the corresponding joint trajectories are C 3 . Consequently, smooth joint movements can be achieved and mechanical solicitations can be reduced. The computational burden of the novel primitive is particularly light, so that trajectories with many via-points can be easily processed even in real-time contexts. As shown in the letter, despite the orientation planner was initially conceived for applications involving industrial manipulators, it could also be profitably used for the management of CNC machines with more than 3 axes. The approach is evidently scalable, so that a future extension of the work could consider the generation of continuous-snap signals obtained by combining G 4 path curves with C 4 orientation primitives. Another potential future improvement of this work could concern the achievement of trajectories with a specified degree of smoothness, obtained by bounding joint velocities, accelerations, and jerks. Such result can be achieved, for example, by means of scaling techniques acting on the timing law [23].