A Cartesian-Based Trajectory Optimization with Jerk Constraints for a Robot

To address the time-optimal trajectory planning (TOTP) problem with joint jerk constraints in a Cartesian coordinate system, we propose a time-optimal path-parameterization (TOPP) algorithm based on nonlinear optimization. The key insight of our approach is the presentation of a comprehensive and effective iterative optimization framework for solving the optimal control problem (OCP) formulation of the TOTP problem in the (s,s˙)-phase plane. In particular, we identify two major difficulties: establishing TOPP in Cartesian space satisfying third-order constraints in joint space, and finding an efficient computational solution to TOPP, which includes nonlinear constraints. Experimental results demonstrate that the proposed method is an effective solution for time-optimal trajectory planning with joint jerk limits, and can be applied to a wide range of robotic systems.


Introduction
Presently, industrial robotics has a wide range of applications, including welding, palletizing, grinding and polishing, assembly, and painting [1][2][3]. After decades of research, the problem of time-optimal trajectory planning (TOTP) of robots along specified paths has been extensively studied to optimize operation time and improve the efficiency of automated industrial robot operations [4]. TOTP is based on interpolation and introduces the concepts of constraint and optimization to maximize the performance of the robot and ensure the shortest time, while making the trajectory smooth and the operation run smoothly [5]. Time-optimal path parameterization (TOPP) is a fast method for determining critical conditions for navigating a pre-defined smooth path in a robot system's configuration space while respecting physical constraints [6]. Although finding the time-optimal parameterization of a path subject to second-order constraints is a well-studied problem in robotics, TOPP subject to third-order constraints (such as jerk and torque rate) has received relatively little attention and remains largely open. Moreover, joint space trajectory planning cannot visualize the end position of the robotic arm, and Cartesian space trajectory planning is often used in many specific industrial scenarios such as welding, cutting, or machining that require operation on a predetermined path. Therefore, a TOTP algorithm that satisfies the joint third-order constraints in Cartesian space is urgently needed. even more) orders of magnitude slower than the CO-based method. Additionally, the DP method cannot truly achieve the global optimal point due to the issue of grid precision.
There are several alternative approaches to the TOTP problem beyond the three groups mentioned above [23][24][25][26][27][28]. Nevertheless, these approaches also neglect the thirdorder constraint and do not perform planning in Cartesian space. Table 1 summarizes and compares the similarities and differences of the above three methods in the following five aspects: the requirement for calculating switching points, the ability to consider multiple optimization objectives, the ability to achieve the optimal point (rather than approximately achieving the optimal point), the planning space, and the highest constraint order.

Motivations and Contributions
Motivated by previous approaches, this paper proposes aTOTP algorithm that considers joint third-order limits in a Cartesian coordinate system, maximizing the robot operation efficiency while maintaining smoothness and minimizing time. To achieve this, kinematic feasibility is ensured by introducing joint velocity, acceleration, and jerk constraints on the path parameters s, which are then relocated to the Cartesian space using a constraint transfer method based on Lie theory (We use the Lie group SE(3) to represent the motion of the robot end-effector in Cartesian space. The detailed description of using Lie theory for robot forward and inverse kinematic analysis and the Jacobian matrix derivation process is presented in the Appendix A), reducing the number of decision variables. After establishing the optimal control problem (OCP) formulation of the TOTP problem in the (s,ṡ) phase plane, the TOPP-RA algorithm is extended to the Cartesian space to obtain an initial solution, and a constraint relaxation approach is used to simplify nonconvex state-update constraints. The method is validated through simulation experiments on a ROS-based platform and real-world experiments on an actual robot, demonstrating effectiveness, generality, and robustness. This paper makes several contributions to the field of optimal trajectory generation: • A comprehensive and effective framework for iterative optimization is presented to establish the OCP formulation of the TOTP problem, which is described by the path parameter s; • Given an efficient computational solution for computing the nonlinear TOPP in Cartesian space while satisfying third-order constraints in joint space; • Experiments have demonstrated that the proposed method can effectively generate smoother trajectories that satisfy jerk constraints on a wide range of robot systems.
The remainder of this paper is organized as follows. Section 2 outlines the key features of the OCP model used for the TOPP algorithm. In Section 3, we present the Cartesianbased TOPP-RA method and describe the proposed TOPP algorithm based on iterative optimization. Section 4 reports extensive experimental results. Finally, in Section 5, we provide concluding remarks.

Problem Statement
In this section, we establish the TOPP problem as an OCP in Cartesian space, which includes joint third-order constraints and an objective function in the (s,ṡ) phase plane. The details of these constraints will be formulated in the following subsections.

General Description
In a n-dof robot system, the state profiles in configuration space are denoted by x(t) = [q(t);q(t);q(t)], where q ∈ R n represents the configuration of the system. The control inputs u(t) represent the third derivative of the joint angles, ... q (t), in configuration space. The following is a standard OCP that can be used to describe the time-optimal speed planning problem [29]: (1) f Status−update = 0 forms the status-update process. [x min , x max , u min , u max ] describes the allowable regions of state and control profiles. [x init , u init , x goal , u goal ] denotes the start and end conditions of the state and control profiles. T represents the total time, which is unidentified now.
To translate the above model to a TOPP problem in the (s,ṡ) phase plane, we propose a function p(s) s∈[0,s end ] that represents a geometric path in the Cartesian space, and is piece-wise C 2 -continuous. We introduce a time parameterization that itself represents the parameter of the path, as a piece-wise C 2 , increasing scalar function s : The trajectory is then recovered as p(s(t)) t∈[0,T] [30]. In the rest of this section, we introduce how to complete the transformation of the TOPP problem through s : [0, T] → [0, s end ].

Objective Function
To minimize the total time of robot movement, the objective function J(x(t), u(t)) is defined as Replace the previous equation with ds/ds = 1 and change the integral limits from [0, T] (time) to [0, s end ] (s) [31]. Formula (2) is updated as follows: Therefore, to minimize the time,ṡ −1 (s) should be as small as possible. In other words, s(s) must be as large as possible while still satisfying the various constraints mentioned later. This means that the state trajectory must follow the boundary of the phase diagram plotted by (s,ṡ), which is naturally aligned with TOPP-RA method.

Constraints
In a TOPP problem, there are generally three types of constraints: status-update constraints, constraints on the states/control profiles, and two-point boundary constraints [32].

Status-Update Constraints
The state-update/kinematic constraints of a robot describe the kinematic feasibility of the robot's motion. Using forward and inverse kinematics, the configuration q in the joint space can be converted to the corresponding Cartesian space representation p (see the Appendix A for transformation method). As a result, the state and control profiles can be expressed in terms of the geometric path p(s), which can then be further transformed to a form represented by path parameters s, as shown in Equation (4).
The status-update function can be rewritten by performing a second-order Taylor series expansion at s i .ṡ The error of the first-order status-update discretization function, denoted by e state f irst , is as follows: Similarly, by performing a third-order Tylor series expansion at s i , the second-order status-update discretization function and its error can be, respectively, rewritten as:

States/Control Profiles Constraints
The state/control constraints of a robot refer to the physical constraints that the robot must adhere to during its motion process. Typically, these constraints involve the robot's state variables, such as position, velocity, acceleration, joint angles, and so on. The constraints on the robot's states and control profiles can be formulated as x min ≤ x(t) ≤ x max and u min ≤ u(t) ≤ u max , respectively, where t ∈ [0, T]. These constraints essentially limit the speed, acceleration, and jerk of the robot's joints [33], as illustrated in the following equations.
The derivatives of the joints are projected into Cartesian space through the Jacobian matrix, as shown in Formula (11), which yields the derivatives of the path parameter s.
where is defined as the differentiation of with respect to the path parameter s. Henceforth, we shall refer to s,ṡ,s, and ... s as the position, velocity, acceleration, and jerk, respectively. By substituting Equation (11) into Equation (10), the inequality constraints on the states/control profiles can be expressed as follows: The formulas for calculating each order derivative of the Jacobian matrix (J , J ) will be presented in the Appendix A.

Boundary Constraints
Boundary constraints refer to the limitations imposed on the state and control variables of a robot during the initial and final stages of its operation. The constraints x(0) = x init , u(0) = u init , x(T) = x goal , and u(T) = u goal define the boundary conditions. These boundary conditions ensure that the state and control profiles at the start moment s = 0(t = 0) and the end moment s = s end (t = T) represent the necessary facts at those moments, respectively.
In particular, more degrees of freedom are allowed in setting the control profile ... s at s = 0(t = 0) to ensure the normal operation of the motor.
As a summary of this section, the following OCP is established to represent the TOPP problem based on Cartesian space: s.t. Status-update constraints (4); States/Control profiles constraints (12) and (13); Two-point boundary constraints (14). (15) In general, when moving from the initial state to the target state along a predetermined path, speed planning aims to resolve any potential conflicts that may arise between kinematics-based constraints and environmental constraints. However, due to the nonlinear relationship between the state and control variables, an appropriate initial solution is required to solve the OCP (15). There is a problem with the state/control constraints (12) in OCP (15) because the jerk of the robot is not taken into account when solving the initial solution, which can easily lead to leaving out the free space required for kinematic feasibility. Therefore, directly solving OCP (15) may not always be effective. An alternative option we propose is to build an iterative framework in which the kinematic feasibility is adaptively adjusted when it is found to be inappropriate. The details on how to find an effective computational solution to TOPP with nonlinear constraints are described in Section 3.

TOPP by Iterative Optimization (TOPP-IO)
This section introduces our proposed Cartesian-based TOPP-IO method. First, we present the initial guess and control group generated by the Cartesian-based TOPP-RA method, followed by an explanation of the principle of the TOPP-IO method.

Cartesian-Based TOPP-RA Method
Combining with [6], we expanded the TOPP-RA method from joint space to Cartesian space, which we call the Cartesian-based TOPP-RA method. The geometric path in Cartesian space, denoted by p(s), is divided into N segments with N + 1 grid points, where (s i ,ṡ i ,s i , ... s i ) represents the i-th stage state and control profiles, with i ∈ [0, 1, . . . , N]. The constraints of joint acceleration can be formulated as follows, by taking into account (12) and (13): The velocity constraints of the joints are expressed as a range of i-stage state variables, , which reflects the allowable velocity of the joints.
where j is the j-th element of a(s i ),q min andq max . The state-update function for constant acceleration over [s i , s i+1 ] is given by: where ∆ = s i+1 − s i .

Backward Pass
In considering the segment [s i , s i+1 ] and assuming that the i +1-th feasible range, S i+1 , is known, the i-th feasible range, , can be calculated using the following formula: Obviously, Formula (19) indicates that for anyṡ 2 i ∈ S i , there always exists a statė s 2 i+1 ∈ S i+1 that corresponds to it. In other words, we can always move from the feasible range S i to S i+1 using the state-update function. By applying Formula (19) recursively, we can obtain a set of transitive feasible ranges, [S 0 , S 1 , . . . , S n ]. Any state that belongs to the transitive feasible ranges can be transferred to the ending state when the last feasible range set is determined.

Forward Pass
By transferringṡ 2 i ∈ S i from step i toṡ 2 i+1 ∈ S i+1 of step i + 1, we can recursively reach the final state S n . Furthermore, literature [6] has demonstrated that the transition process occurs on a convex polygon. Therefore, selecting control variables that can reach the upper limit of the next S will result in the shortest task time. This selection exhibits locally greedy behavior while globally optimizing performance. Once the transitive feasible ranges have been derived from the backward pass, the method for transferring (ṡ 2 i ) * to (ṡ 2 i+1 ) * using a greedy algorithm is as follows: where (ṡ 2 i ) * denotes the optimal solution at the i-th grid point. By setting deterministic values of (ṡ 2 0 ) * ∈ S 0 and S n = {(ṡ 2 n ) * }, the solution of Cartesian-based TOPP-RA, [(ṡ 2 0 ) * , (ṡ 2 1 ) * , . . . , (ṡ 2 n ) * ], is obtained by recursively applying Formula (20).

Principle of the Proposed TOPP-IO Method
The general principle of the TOPP iterative optimization method is illustrated by the pseudo-codes in Algorithm 1. Given a path P in Cartesian space, Algorithm 1 first generates an initial conjecture using the ToppraGuess() function to numerically solve (15) without joint jerk limits. This initial conjecture includes the path discretization, all the parameters required to solve (15), and the initial values of all the decision variables. Then, using the full content of this initial conjecture, Algorithm 1 establishes an iterative OCP where an intermediate optimal solution is obtained from each iteration. After the first three lines of initialization, the while loop is applied to iteratively solve the TOPP OCP . Similar to (15), the only difference is that we add (4) as a soft constraint to the objective function. Specifically, this iterative OCP solves the following optimization problems.
States/Control profiles constraints (12) and (13); Two-point boundary constraints (14). (21) where ω si f t > 0 is a parameter used to weight the softening of the state updating, and f so f t (s) is denoted as In each iteration of the while loop, the function SolveIteratively(OCP TOPP , G) is used to solve (21) using the initial conjecture G. The function StateUpdateInfeasibility(G) evaluates the infeasibility degree of the status update determined by f so f t (s) as given in (22). When f so f t (s) becomes small enough, i.e., close to 0 + , the function GetTrajectoryInformation(G) is called to extract information about the optimal trajectory from the solution G.

Algorithm 1: An Iterative Optimal Method for TOPP
Input: Geometric path in Cartesian space P Output: Optimal trajectory information info opti if f so f t (s) < e so f t then 8 info opti ← GetTrajectoryInformation(G) ; 9 return; 10 else 11 ω so f t ← ω so f t · α, iter ← iter + 1 ; 12 end 13 end 14 return;

Properties Discussion of Algorithm 1
This subsection describes the relevant properties of the proposed TOPP-IO method in Algorithm 1.
First, the iterative process progressively increases the feasibility and optimality of the phase state. It is assumed that the initial solution obtained by the Cartesian frame TOPP-RA does not satisfy the jerk constraint and, hence, is not status-update feasible. In such cases, restoring status-update feasibility becomes the primary goal of minimizing the objective function of OCP TOPP . Therefore, the optimal solution differs from the initial guess by reducing the status-update infeasibility. Although the status-update infeasibility may not be eliminated, the resulting (s,ṡ) phase diagram is closer to being feasible, providing opportunities for further improvement in succeeding iterations.
Second, optimality is achieved when Algorithm 1 exits from line 9. As the iteration continues, the status-update infeasibility approaches 0 + and incrementing ω so f t expedites the procedure. When the degree of status-update infeasibility is small, the total time (3) in the objective function of (21) dominates. Thus, the objective function of (21) is minimized, closing in on minimizing the original objective function (3) to an accuracy level of e so f t .
Third, the OCP TOPP is always feasible, which is a crucial cornerstone of the entire iterative framework. With strict restrictions on CPU runtime and a willingness to accept suboptimal solutions, a feasible solution can be obtained at any point by interrupting the iterative optimization process. With very slow motion always feasible, the solution procedure for each (21) is consistently in the feasible region of the solution space when the initial solution is set to 0. Thus, as long as the obtained (s,ṡ) phase diagram's near-future period is status-update feasible, the resulting phase states can be transferred to the next iterative OCP TOPP for further enhancements.

Simulation and Real-World Experiment Results
In this section, simulation experiments will be used to demonstrate the feasibility, performance, and generality of the proposed method, as well as an industrial robot real machine-verification experiment will be performed, which gives practical significance to the TOPP-IO algorithm. The proposed method is executed on Ubuntu using an Intel i7-7700HQ @ 2.80 GHz CPU and 16-GB RAM, and all optimization problems are solved using CasADi (CasAdi is an open-source software framework for nonlinear optimization and optimal control. It provides a flexible and efficient interface for constructing and solving various optimization problems, including trajectory optimization) [34]. We use the 6-DOF Firefox robot from SIASUN in both the simulation and the real world, in addition to the Pioneer P3-DX robot used in the simulation. The implementation of TOPP-IO was done in C++, and the required communication between systems for these experiments was established. Figure 1 illustrates the architecture of the implementation.

Experiment Settings
The joint and wheel velocity, acceleration, and jerk limits are presented in each experiment, respectively, which are critical factors for the safe and efficient operation of robotic systems. To assess the robustness and adaptability of our proposed algorithm, TOPP-IO, we conducted a series of experiments with varying jerk limits. Specifically, we evaluated the performance of TOPP-IO under four different jerk limits: 0.1×, 1×, 10×, and 100× the default value. The basic parameters for the iterative optimization are carefully selected to ensure the convergence and efficiency of the optimization process, which are listed in Table 2.

Comparison with TOPP-RA Method
This method is built and tested on the random geometric route depicted in Figure 2, subject to joint velocity, acceleration, and jerk limitations which are presented in Table 3. The simulation results are compared with those obtained from the CO algorithm (TOPP-RA) presented in [6] to demonstrate the effectiveness of the proposed strategy in controlling the acceleration surge caused by ignoring the jerk constraints.  The results of the two approaches, TOPP-RA and TOPP-IO, in the (s,ṡ) and (s,s) phase planes are presented in Figures 3 and 4, respectively. It can be observed from Figure 4 that TOPP-RA allows for steep slopes of acceleration due to the lack of restriction on jerk, leading to an abrupt shift in acceleration between neighboring path points. This sudden change in acceleration can be seen in the velocity curve of Figure 3, where there is no smooth transition between the acceleration and deceleration portions. Such abrupt changes in acceleration can result in jerky and unstable motion, which is not desirable in many real-world applications. To address this issue, TOPP-IO imposes explicit joint jerk limitations, leading to smoother acceleration profiles between neighboring path points. Figure 4 shows that the TOPP-IO method successfully restricts the acceleration mutation, preventing any abrupt changes in acceleration. Furthermore, the velocity curve of TOPP-IO in Figure 3 exhibits smoother transitions between the portions representing acceleration and deceleration, guaranteeing that the nearby segments will not violate the imposed restrictions.  To further evaluate the performance of the two approaches, we compare their execution times in Table 4 and display the corresponding speed, acceleration, and jerk curves in Figure 5 for various jerk limits (100×, 10×, 1×, and 0.1×). In the TOPP-RA method, it is evident that the acceleration profiles are bang-bang, satisfying all joint second-order constraints.
With all third-order kinematic constraints, the jerk profiles are bang-bang in the TOPP-IO method, leading to smoother transitions between the portions representing acceleration and deceleration. Without joint jerk limits, the maximum acceleration is about 1638.4 rad/s 3 . As the jerk limit is decreased from "none" to 100× and 10× jerk limits, the execution time only slightly increases from 2.81067 s to 2.89393 s and 2.90326 s, respectively, and the smoothing effect of the speed profile is not immediately noticeable. The speed profile becomes smoother as the jerk limits approach 1× jerk limits. Notably, even when the jerk limit is set to 0.1× jerk limits, TOPP-IO can still produce a valid solution, albeit with an increased execution time.

Application on Mobile Robot
Our method applies not only to manipulators but also to a wide range of robots. To demonstrate its flexibility, we computed a ground trajectory for the Pioneer P3-DX, a diff-drive mobile robot. The wheel velocity, acceleration, and jerk limitations are presented in Table 5. Screenshots of the operational phase as well as the wheel speed curve in comparison to TOPP-RA are shown in Figure 6. Table 5. Velocity, acceleration, and jerk limits of wheels.

Limits Wheel1 Wheel2
Vel. (rad/s) 2 2 Acc. (rad/s 2 ) 4 4 Jerk (rad/s 3 ) 8 8 The restrictions on wheel jerk and route jerk constraints have similar effects on controlling acceleration mutation. In this study, jerk restrictions were defined as wheel jerk constraints that effectively limit acceleration mutation in the route. The robot trajectories obtained from TOPP-RA and the proposed method are presented in Figure 7. Table 6 indicates that the maximum absolute values of the robot's acceleration and jerk curves obtained from the proposed method are reduced by 60.28% and 69.82%, respectively, compared to those from TOPP-RA.
(a) Cartesian space (b) Wheel space Figure 6. The ground path on which this approach is implemented and tested.

Real-World Experiments
In real-world experiments, we applied our method to the welding industry where the objective is to complete tasks as quickly, safely, and efficiently as possible. TOPP-IO succeeded in executing the assignment in a timely, safe, and stable manner. The running state of the Firefox robot in the actual world is shown in Figure 8 and is consistent with the simulation results. We performed both quantitative and qualitative analyses of our method's performance during the actual operation process. Specifically, we analyzed the position error of each joint and examined the speed-tracking situation using joint1 as an example. Figure 9a shows the joint position error of the TOPP-RA method during actual operation, while Figure 9b displays the joint position error of the TOPP-IO method under the same path. In addition, Table 7 compares the performance of our TOPP-IO method with that of the TOPP-RA method. The results show that the average and maximum position errors of all joints in TOPP-IO have been reduced to different degrees during operation. The absolute values of the average position error and maximum position error have been reduced by about 29% and 27%, respectively, compared to the TOPP-RA method. To examine the speed-tracking situation, we used joint1 as an example. Figure 10a shows the speed-tracking of the TOPP-RA method during actual operation, while Figure 10b presents the speed curve of our TOPP-IO method, which considers the third-order constraint. Only the second-order constraint of joint space is considered, which leads to snap-point (represented by the gray circle), or the sudden change in joint acceleration, resulting in the inability to track the given speed on the actual physical robot. As shown in the zoomed-in section of Figure 10a, the snap-point causes fluctuations in the speed curve. In contrast, our TOPP-IO method eliminates the snap-point and enables smooth tracking of joint speed. Our method ensures a smooth trajectory and efficient, steady completion of the task while maintaining high speed.

Conclusions
In this paper, we develop a comprehensive and efficient iterative optimization framework for solving the TOTP problem with joint third-order constraints. The main contributions and results of this paper are as follows: • The framework is constructed from the bottom up in the Cartesian coordinate system and can be applied to both manipulator and mobile robots; • Our study has identified two main challenges in the framework: how to consistently represent the TOTP problem in the Cartesian space using the (s,ṡ) phase plane, while imposing third-order kinematic constraints on each joint, and how to devise an efficient computational solution strategy that uses a constraint relaxation approach to simplify nonconvex constraints without violating them; • We demonstrated the effectiveness of our proposed framework through both simulation and physical experiments. Compared to the TOPP-RA method, our approach effectively reduced the maximum absolute values of the robot's jerk and the average absolute values of the position error over 60% and 29%, respectively. These are critical factors in ensuring smooth robotic velocity tracking and reducing impact during operation.
Our framework has a few limitations. First, we assume that the path of the end-effector in Cartesian space is predetermined. We use B-spline interpolation to generate continuous, smooth end-effector poses from the given path points. Second, our approach accepts suboptimal solutions when a feasible solution can be obtained at any point by interrupting the iterative optimization process.
Our future work can be divided into two main areas: • First, we aim to extend our framework to handle both path planning and speed planning simultaneously, which will enable our method to generate feasible solutions more efficiently; • Second, we plan to explore the potential of the constraint relaxation approaches and achieve real-time performance. Moreover, handling dynamic environments is a challenging and interesting area for future research. Institutional Review Board Statement: Not applicable.

Informed Consent Statement: Not applicable.
Data Availability Statement: Not applicable.

Conflicts of Interest:
The authors declare no conflict of interest.

Abbreviations
The following abbreviations are used in this manuscript:

Appendix A. From Configuration Space to Cartesian Space
In this appendix, we introduce how to determine the position and attitude of the end-effector and their derivatives using the robot joint variables and their derivatives of each order, and then convert them into path parameter s for representation. This process involves forward kinematics, inverse kinematics and the derivatives of Jacobian matrix, which will be described in detail in the following subsections.
Appendix A.1. Forward and Inverse Kinematics ψ is defined as the screw coordinate of the spiral axis relative to the spatial coordinate system. Its Lie algebra representation is as follows: The Lie group corresponding to ψ can be expressed as where R and separately denote the directions and positions of the rigid-body. The mapping between the Lie algebra ψ and the corresponding Lie group Ψ is given by: Consider a robot system with n degrees of freedom, whose configuration is represented by q = [q 1 , q 2 , · · · , q n ] T ∈ R n . The forward kinematics model of the robot is determined as follows: where Ψ S represents the pose matrix of the end coordinate system relative to the space coordinate system when the robot is in the initial position; (q i , ψ i ) denote the joint position and twist, respectively, of the i-th joint; R end and end separately denote the orientations and positions of the end-effector.
For a given robot model, it is possible to convert the end pose p expressed in Cartesian space to the corresponding joint values q in joint space using inverse kinematics. Similarly, the joint values q can be converted to the end pose p through forward kinematics. Thus, the reciprocal transformation between p and q can be achieved through these two transformations.
(1) First-order path parameter derivative J : The first-order derivative of Jacobian matrix J with respect to the path parameter s is as following: The matrices J p and J p depend on the path parameter s, while J b is a function of the joint values q, which can be obtained by forward and inverse kinematics. Each column of J b (q) can be represented as an adjoint matrix, given by: According to the chain rule of differentiation, the first-order derivative of the geometric Jacobian matrix with respect to the path parameter s, denoted as J b , and its i-th column, denoted as β i , can be expressed as: In combination with the literature [35], ∂β i ∂q j can be calculated using (β i , β j ) as follows: (2) Second-order path parameter derivative J : The second-order derivative of the Jacobian matrix J with respect to the path parameter s is given by: According to the chain rule, the second-order path parameter derivative, J b , and its i-th column, β i , can be denoted as: where q = J −1 (p − J J −1 p ) (A14) and ∂ ∂s Overall, this appendix establishes the forward kinematics of a robot using Lie theory and symbolically derives the first-order and second-order derivatives of the Jacobian matrix. Higher-order Jacobian matrices could be derived similarly. Furthermore, the inverse kinematics for a specific robot model can be easily obtained.