An Online Trajectory Generator on SE(3) for Human–Robot Collaboration

Summary With the increasing demand for humans and robots to collaborate in a joint workspace, it is essential that robots react and adapt instantaneously to unforeseen events to ensure safety. Constraining robot dynamics directly on SE(3), that is, the group of 3D translation and rotation, is essential to comply with the emerging Human–Robot Collaboration (HRC) safety standard ISO/TS 15066. We argue that limiting coordinate-independent magnitudes of physical dynamic quantities at the same time allows more intuitive constraint definitions. We present the first real-time capable online trajectory generator that constrains translational and rotational magnitude values of 3D translation and 3D rotation dynamics in a singularity-free formulation. Simulations as well as experiments on a hardware platform show the utility in HRC contexts.


Introduction
Enabling humans and robots to physically work together on cooperative tasks in close distance is a very active area of current robotic research. In such HRC scenarios, a trajectory generator must not only assure human safety but at the same time respect human comfort, in order to increase acceptance of the robot by human co-workers. The former-safety-asks for a real-time capable trajectory generator that reacts instantaneously to sensor inputs or a change in constraints. This requires evaluation of new set points in every embedded low-level iteration cycle at high frequencies. While literature offers various solutions for online trajectory generators (OTGs) with axis-specific constraints on different derivative orders, the latter-comfort-favors constraining velocities and accelerations directly in the most intuitive context for the human, namely, 3D translation as well as 3D rotation in their geometric sense, that is, vectors with an orthonormal bases. Rather than limiting dynamics of the coordinate-wise components independently, human presence further suggests restricting end effector movement dynamics in their absolute values.
Restricting the magnitude of translational velocity is in particular essential for incorporating safety standards in HRC according to ISO/TS 15066 1 . Although this international standard is still under development, it serves as a guideline on how safety during collaborative operations must be provided. Possible methods listed in this standard are safety-rated monitored stop, hand guiding, speed and separation monitoring as well as power and force-limited collaborative operation. Especially for collaboration in close distance, only the latter is an appropriate option. Within this method of operation, contact events are categorized in quasi-static and transient contact. The former includes clamping or crushing situations and mainly addresses the robot controller. The latter treats dynamic impact hazards, which can be actively considered in OTGs. Safety is provided by limiting the transferred energy a Only true for ref. [12]. b Only true for ref. [18]. during impact, according to different human body regions. Given the effective mass of the two-body system, one can derive the maximal allowed relative speed between the robot and the human body region. Detailed calculation guidelines are outlined in. 1 Considering these facts, we propose the following requirements for an OTG in HRC contexts: R1: Translation and rotation dynamic quantities are constrained in their magnitudes. R2: Rotation dynamics refer to geometric angular quantities. R3: All degrees of freedom (DOF) are synchronized in their motions. R4: Constraint-conform trajectories are directly forwarded without delay. R5: Iteration cycle times below 1ms must be guaranteed.
Fulfilling these requirements is not addressed in current state-of-the-art OTGs.

Related work
Current OTGs 2 are able to make robotic systems robust against infeasible trajectory inputs due to, for example, step inputs, improper path transitions caused by a potential higher level path planner, or possible communication delay fluctuations. Online capability also enables instantaneous reaction to sudden unforeseen events. 3 However, authors usually either consider trajectory generation in joint space while moving close to hardware limits to achieve time optimal solutions, or extend these approaches to end effector movements by treating the translational and rotational DOF as independent axes in R n . Due to the demand for instantaneous reaction to unforeseen events (R5), pure path planners without temporal information as well as pure path parametrization methods are not considered in this short review. We classify the relevant OTGs approaches into three major groups: Direct approaches define a trajectory profile and precompute the whole trajectory to the target state. Most trajectory generators are based on piecewise polynomials as they can be easily designed for arbitrarily often differentiable trajectories. Many approaches, however, are restricted to zero dynamic start and/or end conditions, which entails a constant time delay when feeding the algorithm with already feasible trajectories. 4,5 Kröger et al. 3 provided a basic concept for OTGs and also introduced a classification scheme. Katzschmann et al. 6 extended the approach to regard the entire robot dynamics. Ezair et al. 7 proposed an iterative approach, that uses recursive S-curve polynomials to generate trajectories of arbitrary order with general initial and final conditions. Another iterative algorithm that considers a sampled input trajectory was proposed by Lange et al. 8 and later improved for fast trajectories in ref. [9]. Their work focuses on path-accurate OTG. These approaches are designed for application in joint space. Although the joint trajectories could be derived from an analogous Cartesian trajectory, only constraints in joint space are considered. Kröger expanded the ideas from 3 to straight motions in 3D Euclidean space in [10] by considering orientation in the minimal representation by Euler angles. However, the derivations of these values do not lead to geometric angular velocities, but are merely of analytic nature as they represent concatenated velocities. This important difference will be further discussed in Section 5.2.
Rymansaib et al. 11 propose the usage of exponential functions rather than polynomials for trajectory generation. This allows consideration of hardware constraints as well as generation of trajectories with continuous jerk. They show that the exponential trajectory generation results in higher tracking accuracy than common S-curve velocity methods. Nevertheless, their approach is also designed for pre-planned point-to-point (PTP) motions with zero target velocities/acceleration, and therefore suffers from time delays as well, which conflicts with R4. Moreover, none of the abovementioned direct methods can fulfill R2. The only to the authors known OTG that does formulate geometric angular velocity constraints on SO (3), that is, the group of 3D rotations, is an unpublished work by Lloyd. 12 He reduces the 3D problem to two 1D subproblems by transformation to a locally radial and thus easily solvable coordinate system. Due to that decomposition, however, the approach does not comply with R1 and further does not allow desired velocity set points, necessary for R4.
Indirect approaches consider the trajectory generation as dynamic control system problem, that is, a chain of integrators. Filter-based techniques were adopted by Biagiotti et al. 13,14 and Besset et al. 15 The trajectory generators proposed by Besset et al. use finite impulse response (FIR) filters to generate jerk-limited profiles out of initially acceleration-limited trajectories. While a pregenerated trajectory is given in advance, it delivers a solution in less than 1 µs and thus is the fastest time optimal jerk-limited trajectory generator at this time, even for the multidimensional case. Biagiotti et al. 13 use a chain of FIR filters to smoothen trajectories to have continuous derivatives of arbitrary order. In [14], the strategy is generalized to produce piecewise exponential jerk profiles, to further reduce machine vibrations. Due to their filtering nature, these approaches again result in time delays and thus neither satisfy R4. Gerelli et al. 16 and Bianco et al. 17 proposed a discrete-time filter that incorporates constraints as sliding surfaces. These inspiring works generate time optimal trajectories under consideration of bounded velocity, acceleration, and jerk for single-DOF applications. In [18], Bianco proposes a strategy to synchronize multiple DOF. Besides fulfilling R4, the latter extends the approach to further comply with R3.
Optimization-based approaches rely on numerical solvers. Ardakani et al. 19 suggest a real-time joint trajectory generator based on model predictive control. Their formulation on R n achieves an impressive 200 Hz sampling rate for a 6DOF robot kinematic despite the optimization procedure in every iteration. Dinh et al. 20 combine sequential action control 25 with indirect optimization. In numerical optimization frameworks, it is also possible to generate trajectories for hybrid dynamic systems such as the table tennis robot in Koç et al. 21 While solving the individual optimization problems with sequential quadratic programming requires over 1s computation time, they precompute a lookup table from a fixed initial posture that can be used online. However, for a more general trajectory generation problem, this approach is infeasible. Gao et al. 22 use a rapidly exploring random graph method, for finding a collision-free trajectory for a quadrotor in complex environments. Note that formulating the kinematics in R n , which holds, for example, for joint trajectories as well as 3D translation, results in a linear chain of integrators. Optimization of 3D rotation trajectories on the other hand result in highly nonlinear and coupled dynamics. Le et al. 23 study the time optimal control problem for SO(3) in a general setting, but only include acceleration constraints. Another algorithm for trajectory generation directly on SE(3) is proposed by Watterson et al. 24 They use semi-definite programming techniques and also consider obstacles in the environment. Using numerical optimization techniques is usually a limiting factor when hard real-time capabilities as the one in R5 are required, especially when nonlinear constraints and SO(3) dynamics (R1) are introduced. None of the above-mentioned work was designed for scenarios that impose requirements R1 and R2 while being real-time capable as required by R5. Table I gives a concise summary of the referenced approaches w.r.t. the posed requirements.

Contribution
In this article, we significantly refine our method from ref. [26] with a rigorous treatment of the problem for 3D orientations in SO(3). We develop an algorithm that directly applies the special orthogonal rotation matrices. It is further adapted to unit quaternion representation. Special case singularity treatment as well as consideration of multiple goal states are hence not necessary, in contrast to the often used Euler angle representations. The main contributions of this work are: 1. This is the first OTG algorithm that allows constraining the norm of translational and rotational dynamic quantities, essential to comply with safety standards defined in ISO/TS 15066 1 . 2. We introduce the Magnus expansion to OTG treatment. This allows calculating solution to the differential equations on SO(3) with high accuracy.

Outline
The remainder of the paper is organized as follows. The problem of a modular OTG under requirements R1-R5 is formally formulated in Section 2. In Section 3, the approach is first introduced for the translational case and then adapted to the rotational DOF. Finally, it is shown how the two portions can be synchronized w.r.t. time. For a distinct highlighting of the potential of our algorithm in robotic applications, we outline constraint extensions for HRC scenarios in Section 4 and discuss its performance with a comparison w.r.t. to the state-of-the-art OTG algorithms in Section 5. We conclude the work and outline future directions of development in Section 6.

Problem Formulation
Regarding a modular robot architecture of a complex system, a clear distinction between trajectory generation and robot control is of advantage. Therefore, an interface is needed, that guarantees desired trajectories sent to an arbitrary robot platform stay within defined dynamic constraints. Especially in close distance HRC scenarios, it is essential for these constraints to be defined in an intuitive metric, that is, coordinate-independent magnitudes of translational and rotational speed and acceleration. Let the end effector pose of a robot be given in SE(3) := R 3 × SO(3), that consists of a 3D translational position p ∈ R 3 and a 3D orientation R ∈ SO(3). The special orthogonal group SO(3) is defined as SO(3) := {R ∈ R 3×3 |R T R = I 3×3 , det(R) = 1}, with I 3×3 referring to the identity matrix in R 3×3 . The system state vector x at time t k ∈ R consists of the full 6D pose, together with translational velocity v ∈ R 3 and angular velocity ω ∈ R 3 . It is denoted as the 4-tuple x(t k ) = x k := ( p k , R k , v k , ω k ). Applying a constant acceleration tuple k := (a k , α k ) consisting of translational and rotational acceleration a k ∈ R 3 and α k ∈ R 3 , respectively, for a given time span T ∈ R, the system state advances according to where f denotes the OTG-dependent state propagation function. Assuming a discrete implementation of the OTG running at a sample time T s ∈ R, we refer to the state at time t k + iT s with i ∈ N + as x k+i . Given a desired state x des and the current system state x k , the OTG must solve the problem subject to the state progression and subject to the nonlinear user-defined dynamic magnitude constraints for all i ∈ [k, N − 1] time steps. The operator · 2 denotes the 2-norm and thus the magnitude of the vectors.
Note that Le et al. 23 solve a similar problem on SE(3) for a reduced set of dynamic constraints. Their numerical examples show computation times of over 90s for an individual trajectory solution, and thus demonstrate the complexity of the problem at hand. Considering the core requirement of every OTG being fast computation, in order to guarantee safety at typical robot sampling frequencies of 1 kHz, the use of iterative numeric optimization algorithms is not appropriate. Accordingly, we relax the requirement of absolute time optimality and accept close-to-optimal solutions under realtime capable iteration cycles ≤1 ms instead.

Approach
Considering that the dynamic constraints (4) are related to translational and rotational quantities separately, we partition the state dynamics (1) into the individual mappings The translation dynamics are thus given with f p and f v , whereas the rotation dynamics consist of f R and f ω .
Starting from the basic translational problem in Section 3.1, the approach is adapted for the rotational group SO(3) in Section 3.2. Based on these formulations, the synchronization over all 6 DOF w.r.t. to time is outlined in Section 3.3. The set of conditions listed in (4) from the problem formulation is extended to additional HRC motivated use cases in Section 4.

Translation
For 3D translation, the three components can be treated independently if they are defined w.r.t. an orthonormal basis. The desired behavior for the algorithm is a continuously differentiable position trajectory. This is achieved by calculating the next state via integration of the limited acceleration. The discrete system dynamics (3a) for the translational case are three decoupled double integrators, that is, for a given position p and velocity v at time t k , applying a constant acceleration vector a k for the time T advances according to the mappings The aim of the algorithm is to compute the acceleration such that the position error e p,k := p des k − p k as well as the velocity error e v,k := v des k − v k both converge to zero. The minimum time needed to reach v des k is achieved, if the acceleration vector a k in (6a) exploits the acceleration constraint (4b) and points in the same direction as the velocity error e v,k , respectively, v des As the discrete algorithm will run on a fixed sample time T s , and acceleration is constant in between iterations, the minimal time needed is in fact the next multiple of T s , denoted by with operator · being the ceiling function that rounds up to the next full integer. The maximization operator in (8) ensures T min,tra ≥ T s . Requiring to reach the desired velocity v des k = f v (v k ,â k , T min,tra ) in the time span T min,tra is directly used to define the acceleration vector that would result in a synchronous convergence of e v (t k + T min,tra ) = 0 while respecting the acceleration constraint (4b).
Applyingâ k to the position integration mapping f p and imposing to reach the desired velocity If this velocity is matched, the position error e p converges to zero together with the velocity error e v for T → T min,tra . Advancing v goal k for a single iteration leads to and is used as the goal velocity at time t k+1 . The second phase of the algorithm handles given constraints by defining two sets of saturation factors S v ⊂ R + and S a ⊂ R + for velocity and acceleration, respectively.
Constraints on the trajectory generator output are incorporated by multiplication with the most restrictive element of the corresponding saturation factor set. The translational speed constraint (4a), for example, is met by the set definition Note that all factors in the set are normalized and saturation requires the factor 1 to be part of the set. 1 For the goal velocity v goal k+1 , this factorization is applied with where superscript (·) sat denotes a saturated value. The necessary acceleration vector to reach velocity constraint conform v sat k+1 is found by solving Eventually, a constraint-conform acceleration vector satisfying a k 2 ≤ a max is found with using the set definition analogously to the velocity saturation (12). The output of the trajectory generator at hand is ultimately found by advancing the current state Note that the saturation steps for constraint handling do not conflict with the desired continuous differentiability of the pose trajectories, as velocity and position values are obtained by integration of (6) under constant acceleration a sat k until the next iteration cycle. Figure 1 illustrates the mechanism of the approach for a PTP motion. A concise implementation including all necessary steps is outlined in Algorithm 1.
In Section 4, additional saturation factors relevant in HRC scenarios are developed. They are integrated by adding elements to the sets defined in (12) and (16).

Rotation
In the rotational treatment of the OTG lies the main contribution of this work. Many different representation forms are known to represent 3D rotations on the special orthogonal group SO(3). In our preliminary work, 26 we used Euler angles as they form a minimal description unlike axis angle, quaternions, or rotation matrix representation. This allowed a straightforward analogy between the translational and the rotational treatment. However, the well-known gimbal-lock singularities of this representation form require special treatment. Furthermore, there always exist two sets of angles to describe the same orientation. Exploiting this dual representation allows finding shorter paths in case of PTP motions. Because angular velocity ω and acceleration α are both defined in R 3 and thus are geometrically decoupled, the algorithm of the translational case can, to the most extend, directly be adapted for the rotational case, with the exception of f p defined in (10) that incorporates the temporal evolution of the position vector. While position p ∈ R 3 allows elementary integration of the individual components, the same does not hold for the orientation.
Let R ∈ SO(3) be a rotation matrix that describes the rotation of a orthonormal basis from a fixed inertial frame to the body-fixed frame. The angular velocity ω ∈ R 3 describes the angular velocity between the inertial frame and the body-fixed frame w.r.t. the inertial frame. To constrain the angular speed ω 2 in the same fashion as the translational case, we start from the relation between the rotation matrix R and angular velocity ω. It is given by the vector cross producṫ and can be expressed in matrix formṘ where [·] × : R 3 → so(3) denotes the skew operator. Note that [ω] × is also called the SO(3)associated Lie algebra so3. We also use the inverse operator [·] ∨ : so(3) → R 3 that recovers the vector components of a skew symmetric matrix and define it as While (19) suggests an exponential integration of the form this only leads to correct orientations in the case of a fixed rotation axis ω/ ω 2 , due to the definition of the matrix exponential as a power series and the noncommutativity of the elements in SO (3). This leads to various iterative integration schemes, where the axis is assumed to be fixed only for a single small time step T s . See ref. [27] for a detailed derivation and comparison of different schemes.
If (21) is solved for small time steps T, the error that comes from noncommutativity of SO(3) is negligible and thus often used for iterative schemes with small sampling times T s . However, the crucial step of finding the new goal velocity v goal k+1 from (11) is accomplished by solving the time integration over a time span of T min,tra , which cannot be assumed to be small.
The chosen approach in this work is the use of the Magnus expansion as originally proposed in ref. [28]. This method provides an analytical framework for finding the solution of linear, time-variant matrix differential equations such as (19).

Magnus expansion.
Opposed to the iterative integration methods mentioned above, the Magnus expansion gives an exact solution, important for longer time ranges. The reason that (21) is not correct for the general rotational case lies in the noncommutativity of matrices [ω] × . The nonvanishing portion of the commutation is denoted as the commutator and naturally arises in Lie structures such as the Lie algebra g. Magnus' treatment is given in a more general problem; however, our explanation refers to a real-valued time-variant matrix differential equation of the form (19). Magnus' proposal is to find a solution for (19) in the form of a true matrix exponential with a series expansion that is referred to as Magnus expansion. It is an infinite power series, however as pointed out in ref. [29], it is usually sufficiently accurate for applications to truncate the series after the first three terms which are given as Note that all brackets used refer to the commutator defined in (22). For an explicit solution of the fourth-order term as well as a recursive scheme to calculate Magnus expansion terms of arbitrary order, we refer to ref. [29]. The first three terms of the Magnus expansion (24) can be calculated explicitly for our system (19) and read The operator [·] × again denotes the skew symmetric matrix operator defined in (19). The Magnus expansion can thus be calculates as where I 3×3 is the identity matrix.

OTG for SO(3) using rotation matrices.
The system dynamics of the rotational portion can now be stated as where we see that unlike in the translation mapping f p , the orientation mapping f R is nonlinear and strongly coupled. Analog to the algorithm for translation, the minimum time needed to drive the rotational velocity error e ω,k := ω des k − ω k to zero is Discretizing the value w.r.t. the given sampling time T s reads The corresponding angular acceleration iŝ and is analog to v goal k from (10) used to find the new goal velocity ω goal k by solving which in comparison to our treatment with Euler angle representation in ref. [26] is completely singularity-free and unambiguous. The proof that M is invertible for any t = 0 is provided in the appendix. The goal velocity for the next time step is again found by advancing the previous equation for a single iteration The constraints (4c) and (4d) are again incorporated by defining saturation factor sets S ω ⊂ R + and S α ⊂ R + for the angular velocity and acceleration, respectively. The saturated rotation velocity is thus found by using the factorization set definition This again leads to the find goal acceleration Similarly, defining the acceleration saturation set and applying it to the goal acceleration results in the saturated angular acceleration α sat k . This acceleration vector is eventually applied to the SO(3) system dynamics (34), to advance the current orientation states which concludes the algorithm using rotation matrices.

OTG for SO(3) using unit quaternions.
The above algorithm can be straightforward translated to unit quaternion representation of SO (3). This reduces computation time, as discussed in Section 5.
Let Q ∈ H describe the 3D orientation as a unit quaternion, that is, Q 2 = 1. If we consider a quaternion Q := (Q w , Q v ) consisting of a scalar part Q w and a vector part Q v , the angular velocity ω can be expressed as a pure quaternion (0, ω), that is, with zero scalar part. We introduce the mappings to specify notation. With this notation, the differential equation relating time derivatives of Q with the angular velocity ω readsQ Magnus' idea to solve the differential equation results in solving (49) in the form Note that unlike in the Matrix version of Magnus' proposal from (23), does not occur with the skew symmetric matrix operator [·] × . The Magnus expansion of and its approximation (32), however, remain the same. The mapping that advance the current quaternion in time, hence is The definition of M in (33) as well as velocity progression (34a) are unchanged. Therefore, the only adjusted equations for the algorithm outlined in Section 3.2 are the definition of ω goal k (38) w.r.t. quaternions and consequently, The algorithm for OTG on rotations using quaternions is listed in Algorithm 2.

Synchronization of translation and rotation
While T min,tra and T min,rot from (7) and (36) assure time synchronization within the translational and rotational DOF, respectively, the two groups are not yet synchronized with each other. Complete synchronization of the whole 6D movement can be achieved by coupling the translation and rotation at two distinct places in the algorithm. First, the more restrictive minimum time is used for finding both acceleration vectors a k and α k in (9) and (37). Second, the saturation of the goal velocities are matched by combining their saturation factor sets (12) and (42) as An example of this synchronization is shown in Fig. 2.

Extensions for HRC Scenarios
While the presented algorithm considers a free 6D motion of, for example, the tool center point (TCP) of a robot, it does not depend on any specific robot kinematic. In the following section, we present some interesting constraint extensions particularly relevant in HRC context. The examples highlight the flexibility of our approach and demonstrate the straightforward integration for additional requirements by adding appropriate factors to the saturation sets S. The factors are found via a three-step process: Step 1 formulate a scalar inequality condition, for example, Step 2 derive the normalization factor μ that yields equality to 1, for example, An online trajectory generator on SE (3)  Step 3 add the factor to the corresponding saturation factor set, for example, Note that in the following, all constraints refer to the variables at the next discrete-time instance k + 1. Therefore, we suppress the discrete-time index k + 1 in the remainder of this section for better readability. Further, Sections 4.2-4.4 require that translation and rotation are synchronized as outlined in Section 3.3.

Direction-specific constraints
In robot applications, it is often very useful to constrain different directions of movement independently. While the OTGs mentioned in the related work of Section 1.1 offer the possibility to constrain certain axes of the underlying inertial coordinate system directly, our framework can be easily extended to the more general case of incorporating constraints in arbitrary directions. Possible scenarios in HRC that require these type of constraints, are, for example, the dynamic limitation of robot movements toward the human or obstacles. The requirement of constraining the velocity v tra along a given direction vector r dir ∈ R 3 is formulated as where the v tra is projected onto the direction vector r dir . The necessary saturation factor that complies with this constraint is given as and adding it to the velocity saturation set incorporates this constraint in (13) of the algorithm. Note that this strategy does not only allow constraining orthogonal axes, but several arbitrary directions.

Combining translation and rotation constraints
Whenever humans work in close proximity with robots, robot constraints should be defined as intuitive as possible. Besides constraining linear and angular velocity separately as done in Section 3, we outline a strategy to combine the two into a single intuitive constraint.
(b) (a) Fig. 3. Illustration of (a) total velocity of, for example, the robot wrist at a fixed vector r and (b) projection of the angular velocity ω to identify the fastest point r f within a user-defined safety sphere of radius r.

Constraining Total Velocity of Specific Points.
Instead of constraining the angular speed ω 2 ≤ ω max of the TCP directly, it is sometimes more intuitive to constrain the total linear velocity v tot (r) 2 of a given vector r ∈ R 3 , relative to the TCP. The translational and rotational velocities at the TCP is denoted as v tra := v and v rot := ω × r resp. This can be a specific point on the geometric robot-object compound that is fixed to the TCP, for example, the robots wrist as illustrated in Fig. 3(a). Note that all variables in (59) are given in a world-fixed inertial frame I. In case a specific point is given w.r.t. the end effector coordinate system E, it needs to be transformed, that is, where R IE rotates the vector from E to the inertial frame I. Let θ denote the angle between ω and r.
The fact that the magnitude v rot (r) 2 = ω × r 2 = ω 2 r 2 sin(θ) is linear in ω allows us to saturate v tot 2 ≤ v max by calculating a common factor and adding it to the factorization set If n multiple points are to be considered, for example, to outline a convex polytope around the TCP, multiple factors μ tot,i for i = [1, n] can be calculated and added to the factorization sets in the same fashion. Note that for the TCP itself, r is zero by definition and thus v tot = v tra , which is already considered in the basic algorithm (13). Constraining Total Velocity within Safety Sphere. Instead of defining specific points around the TCP, it also possible to define a safety sphere S := {x ∈ R 3 : x 2 ≤ r} with radius r around the TCP. It is then desired that no point within the sphere is allowed to exceed the speed limit v max . This extends the before mentioned strategy to first identifying the fastest point r f within the sphere.
To maximize (61), r must (a) lie on the sphere surface, that is, have length r and (b) be perpendicular to the rotation vector ω. Further, to maximize v tot , the vector r has to be chosen such that (c) the cross product ω × r lies in the plane spanned by ω and v tra , and (d) the angle between v tra and v rot is below 90 • . See Fig. 3(b) for an illustration.
Thus, the fastest point r f ∈ S is given as where r p ∈ {x ∈ R 3 \{0} | ω, x = 0} is an arbitrary vector perpendicular to ω. Its total speed is which leads to the saturation factor (66) Figure 4 shows the effect of this safety sphere constraint, if added to the set and applied in the algorithm.

Robot joint constraints
While the presented OTG algorithm can be directly used to generate 6D trajectories of, for example, the robot end effector, joint velocity limits were not yet considered, as they depend on the robot-specific kinematic structure as well as the inverse kinematics solver in use. Because kinematic relations are usually highly nonlinear due to rotary joints, the inverse kinematic solvers often make use of the linear relationż between task space velocities, for example,ż = v T ,ω T T ∈ R 6 and joint velocitiesq ∈ R n , via the Jacobian J = ∂ż ∂q . If the expressionq = IK(ż) of the inverse kinematics solver in use is known, the joint velocity limits can directly be considered in the algorithm. For demonstration purposes we assume that a damped pseudo-inverse solver of the forṁ is used. The identity matrix I n×n together with the parameter α introduces a damping effect on the inverse kinematics solution. This avoids singularity issues and is discussed in detail in ref. [30]. The individual constraints on the n joint velocities can concisely be written as with the diagonal limit matrix L = diag q 1,lim , . . . ,q n,lim −1 . It contains reciprocal values of the joint-specific velocity limits of all n joints in the serial robot kinematics. The linearity of the velocity mapping (68) again admits a scaling factor such that factorizing the joint velocitiesq is equivalent to factorizing the task space velocities directly. Thus, adding μq to the saturation factor set constrains the joint velocities accordingly. Note, that constraining the joint accelerationq in the same manner is not possible, because the kinematic relation for accelerationsz couples task space velocityż and accelerationz. Thus, a linear relation such as (73), that elevates the joint constraint factor to task space, is not given.

Constraining movement of the whole robot kinematic
In a serial kinematic robot as usually used in a HRC context, the most important movement to constrain is the movement of the end effector or TCP. However, there might be other points on the robot structure that reach even higher velocities. Thus, it is necessary to not only constrain the TCP movement itself, but extend the given speed constrain to every point in the robot kinematic structure in task space. Only then can official safety standards, for example, ISO/TS 15066 1 be satisfied. Given a conventional rigid link structure consisting of joint axes only orthogonal or parallel to the links, such as illustrated in Fig. 3(a), it is sufficient to check the set of axes intersecting points I of the kinematic for v i 2 ≤v max ∀i ∈ I.
The velocities v i are calculated by using the corresponding Jacobian matrices J i with the reduced joint velocity vectorsq. In the illustrated 7DOF kinematic, these are I = {shoulder, elbow, wrist, TCP}. Every constraint leads to a saturation factor that as part of the sets slows down the end effector velocity in the OTG whenever necessary. Note that in the discussed conventional serial 7DOF, this method is only relevant for constraining the elbow movement. The translational shoulder velocity is always zero and can thus be omitted. Further the TCP being the general point of interest is by definition already constrained in the basic algorithm. The wrist can be considered independently of the kinematic structure in use, following the method in Section 4.2.

OTG-independent constraints
Considering time-variant velocity constraints opens up interesting application scenarios too. It allows for integrating OTG-independent metrics to be considered in the OTG algorithm. An exemplary use case in HRC is a distance-sensitive velocity limitation. In terms of safety issues in HRC, the maximal allowed robot velocity in the proposed OTG is made dependent on the Euclidean distance d HR between human and robot using, for example, a Gaussian-shaped weighting term with the shaping factor σ A . A shaping factor of d min 3 , for example, scales the velocity to <1% for d HR → 0. Figure 5 shows the comparison between a fixed v max and the distance-sensitive implementation (80) for the case when the robot crosses 10 cm above the human hand. In this example, it is further demonstrated how the OTG copes with measurement noise of the human position.

Discussion
In this Section, we will discuss the performance of the proposed algorithm in terms of transient behavior. We further give a runtime comparison to the state-of-the-art. Figure 6 shows the response of the approach to several step inputs and time-invariant constraints. Note that for such inputs, the algorithm results in a time optimal bang-bang behavior in acceleration. The user constraints (4a) and (4b) are fulfilled at all times, while (7)-(9) assures time synchronization. In case of trajectory following where the input trajectory fulfills the inequality constraints (4), the algorithm degenerates to

Transient behavior
once the desired state is reached. Thus, in our algorithm, no explicit switching between different strategies is necessary. This means that the algorithm uses axes-synchronized acceleration bang-bang control in cases of constraint violating goal trajectories, no matter if they result from discontinuous set point trajectories or merely from, for example, infeasible velocities. This reduces the error in position and velocity offset and eventually feeds through the target points whenever possible. An example of this behavior is shown in Fig. 7.

Analytic versus Geometric Angular velocity constraints
Current OTG algorithms mentioned in literature do either not provide synchronization treatment of the multidimensional case, or treat all DOF as independent axes for trajectory generation. Directly applying joint-related approaches to Euler angles for representing SO(3)typically are prone to socalled gimbal-lock singularities. As analyzed in Section 3.2, instead of directly regarding constraints for the time derivatives of the Euler angles, we transfer the problem from this analytical subsequent velocity vector to the geometric angular velocities ω. Note that these two velocity vectors differ clearly in their physical interpretation. The time derivative of Euler angles describes consecutive angular velocities according to the given Euler sequence, whereas ω contains simultaneous angular velocity around the base vectors at a given time instance. Constraining these two vectors directly results obviously in different trajectories for a general case. In Fig. 8, we demonstrate the divergence of the generated trajectories for some extreme cases. The closer the configuration gets to the singularity, the larger the discrepancy between Euler angle velocities and actual geometric angular velocities. The differences can be seen in velocity as well as acceleration level. We argue that constraining the angular velocity ω in its magnitude gives raise to more intuitive trajectories than constraining the successive Euler angle velocities.

Runtime analysis
As mentioned in the introduction, optimization-based approaches especially when considering the required nonlinear magnitude constraints as well as dynamics on SO(3) typically fail to deliver results The discontinuous desired profile consists of three distinct segments. An initial ramp with offset (0 -3 s) followed by a constant (3 -6 s) and a sine with distinct frequencies (6 -10 s). State discrepancies at time t = 0 s that usually result in an error state of the robot can be avoided. After the transition phase, the error for translation and rotation (bottom plot) converge to direct feedthrough of the desired trajectory as discussed in (81). Note that in the given Euler angle sequence X(α) → Y(β) → Z(γ ), the gimbal-lock singularity is reached at β = π/2 ≈ 1.57 rad. Dashed lines are desired values/constraints, solid lines reflect our approach, and dash-dotted lines result from the Reflexxes Motion Library. While α and γ accelerate in the same direction at t = 4 s reaching almost double the constrained acceleration limit, constraining the Euler angle derivatives is overrestrictive at t = 7 s due to the opposite movement directions. within common 1ms iteration cycles. Therefore, they cannot be considered for the problem at hand. Representing approaches that adapt methods in R 6 using Euler angle parametrization, we compare our algorithm with the available Reflexxes Motion Library 31 that is based on the work of Kröger. 10 For a fair comparison, the freely available Type II version (unbound jerk) is used. The algorithm was compared in terms of runtime with the OTG of the available Reflexxes Motion Library. Both algorithms were analyzed using the Simulink 8.9 Profiler evaluating 10 6 iterations on a single 3.7 GHz CPU. The resulting computational effort is listed in Table II. Although the implementation and compilation of our algorithm are not explicitly optimized for runtime, the computation time spent per iteration is below the Reflexxes Motion library for all three representations of 3D orientation in SO (3). This makes the proposed algorithm suitable as an OTG or an intermediate layer between a optional higher level trajectory planner, that may run on a slower sample rate, and the robot controller. This increases robustness without large additional computational load.

Conclusion
In this work, a new approach for an OTG was introduced. According to the classification by Kröger et al. 32 our method describes an online trajectory generator OTG of type II (i.e., bounded velocity and acceleration, unbounded jerk, allowing position and velocity targets) in variant B (that is time-variant constraints). It is directed-but not limited-to be used in HRC scenarios, where assuring human comfort and guaranteeing safety are prioritized over time optimality. While offering the typical advantages of OTG, such as increasing robustness against infeasible input trajectories (e.g., PTP step trajectories) and instantaneous reaction capabilities to unknown events, it offers intuitive definition of dynamic constraints. That is, constraining the magnitude of velocity and acceleration vectors of 3D translation as well as 3D rotation. This way the coordinate system-independent dynamics of the end effector, which can also be interpreted as kinetic energies on velocity level, are directly constrained. Further, rotations are constrained in their true geometric angular velocity, rather than purely analytical values such as direct derivation of Euler angles. Accurate time integration of the orientation differential equations is achieved by the means of the Magnus expansion. This is the first real-time capable OTG algorithm to allow such constraints, while directly generating 3D translation and rotation trajectories on SE(3) in a singularity-free formulation. It was also outlined how the set of constraints can be extended to limit the velocity of the full robot structure in joint as well as 3D Euclidean space. Considering time variance of these constraints opens up many possibilities to connect other metrics such as human-robot distance. Our work introduces an algorithm that provides the advantages of an indirect approach, that is, fast computation cycles and instantaneous reaction to unforeseen inputs. At the same time, it also allows seamless transitions to directly forward trajectories that already satisfy dynamic constraints. These transitions do not require an explicit switching or blending between different strategies, but directly result from the algorithm itself measures to the OTG, as shown in experimental evaluation. The beauty of our algorithm lies clearly in its simplicity and its reduced and intuitive definition of the constraints, especially in terms of orientation. The verification of increasing acceptance by constraining magnitudes is still to be validated in user case studies. Suggestions for future development are the extension jerk limitation, which is of importance especially in industrial contexts. This requires a new step in the algorithm in which the correct acceleration profile has to be found before calculating the minimum time left.