Fast Inverse Kinematics Based on Pseudo-Forward Dynamics Computation: Application to Musculoskeletal Inverse Kinematics

Recently, fast and practical inverse kinematics (IK) methods for complicated human models have gained considerable interest owing to the spread of convenient motion-capture or human-augmentation technologies. Although the IK algorithms developed in robotics can also be applied to humans, they experience computational speed issues, especially in real-time applications. This letter presents a new IK algorithm based on the Levenberg–Marquardt (LM) method, LM-PFD (Pseudo-Forward Dynamics), which is remarkably effective particularly in systems with a large degree of freedom (DoF). In the proposed method, the ${\mathrm{O}(N)}$ forward dynamics algorithm is utilized by introducing a virtual dynamical system derived from damping or weighing factors used in the LM method. The letter firstly introduces the basic implementation of LM-PFD for open kinematic chains. Subsequently, an enhanced implementation is presented to address closed kinematic chains, specifically focusing on wire-driven systems. The proposed method was tested on the IK of musculoskeletal models. The computational time of the model with approximately 150 DoF and 300 wires was within 5 ms.

, [5]. In classical IK, when controlling the motion of a robotic manipulator, the joint angle of the robot is calculated to achieve the desired position of its end effector. IK computation can be applied to various redundant and multiple degrees of freedom (DoF) systems that are not limited to robotic manipulators. As it is especially effective for articulated body systems, IK computation is often used for controlling humanoid robots [6], [7] or the motion analysis of humans and animals [8], [9]. For estimating human joint movements from motion capture, for example, in sports or rehabilitation, the IK of a complex human model, including musculoskeletal models, has been studied [10], [11], and efficient algorithms have been developed in robotics attract attention.
Recently, the demand for fast and practical IK algorithms for complex human models has been increasing. The emergence of convenient motion-capturing technologies such as video motion capture has resulted in the collection of large amounts of human motion data [12], [13], [14], [15]. While AI technologies are expected to be applied for various applications related to human musculoskeletal motion, they usually require a large amount of learning dataset on musculoskeletal motion, indicating the massive process of IK computation according to a motion capture dataset. The upcoming development of human augmentation technologies such as robotic assistive devices [16], [17] also requires fast IK algorithms. As robotics devices are often controlled when measuring and estimating human movements, real-time IK computation for the human body has a key role in this field. There are several examples of computing IK of musculoskeletal models in real-time for visual feedback [18]. However, in terms of intervention or assistance toward human motion, the computational speed of these approaches must still be improved to perform real-time IK, for example, within the control loop of robotic devices.
Robotics IK computation usually requires solving nonlinear equations of the generalized coordinates of the kinematic chain, thus achieving a desired position and orientation of the target links. The nonlinear equations can be solved by several numerical methods, such as the Newton-Raphson method, while computing a basic Jacobian matrix [19] derived from the differential kinematics. However, in the case of a large-DoF system, such as humanoids or human models, the IK computation often faces computational issues at the singular posture of the system and when an equation does not admit a solution owing to practical matters, such as measurement noises or infeasible references. For computational stability, the problem of solving nonlinear equations is often treated as a nonlinear optimization problem that minimizes the residual of the nonlinear equations. Several IK methods utilizing optimization algorithms have been proposed [20], [21], [22], [23], [24].
In general, the merits or demerits of the algorithms used in nonlinear optimization depend on the size of the optimization problem. For example, the Levenberg-Marquardt (LM) method [25], which is popular in robotics [4], [5], [21], [23], [24], [26], usually requires the construction of the Jacobian matrix and the solving of linear equations at each iterative computation. The computational complexity of each iteration generally increases in proportion to the third power of the size of the problem. Therefore, in the case of large-sized problems, the gradient-based method, such as the conjugate gradient method or the quasi-Newton method, with appropriate line search algorithms is more convenient owing to the low computational cost of each iteration. The IK approaches based on the gradient-based methods for a large-DoF human model have been proposed [27]. However, the convergence speed at each iterative computation of those gradient methods is usually slow with respect to that of the LM method. In the case of complex human models, further improvements in IK algorithms are demanded to perform IK within a limited time.
This letter presents a new efficient IK algorithm based on LM method, LM-PFD (Pseudo-Forward Dynamics), which is remarkably effective particularly in systems with a large DoF. In the method, the problem of solving IK is treated as the pseudo problem of solving the forward dynamics of a virtual dynamical system. Several studies have made assumptions on the virtual dynamical systems [24], [27] where an assumption is utilized for gradient-based algorithms by introducing virtual springs. The proposed method is mainly aimed at replacing the problem of solving differential IK equations with the forward dynamics (FD) computation [28], [29], [30] for the virtual dynamical systems whose inertial properties correspond to the weighing matrices and damping factors used in the LM method. This replacement allows the utilization of effective O(N ) algorithms developed in the field of FD computation in robotics, thus greatly reducing the computational time per iteration in the LM method.
Based on the concept of LM-PFD, the letter initially introduces the basic implementation utilizing the FD algorithm for open kinematic chains [28]. On the other hand, the motivation of this letter is to achieve rapid IK for human musculoskeletal models, while the method described above can handle only open kinematic chains. Secondly, the enhanced implementation method of LM-PFD for addressing close kinematic chains, particularly for wire-driven systems, is also provided. The efficacy of the enhanced method is demonstrated through solving the IK problem in human musculoskeletal models.
The remainder of this letter is organized as follows. Section II presents the basic concept and implementation of the proposed method. Section III introduces and explains the implementation to a closed kinematic chain, and Section IV presents its implementation in a wire-driven multibody system.

A. Basic Formulation of IK
Let N L be the number of links in the target link system, N J be the number of joints, and q [q 1 T · · · q N L T ] T ∈ R N be the generalized coordinates, and q j ∈ R n j represents the generalized coordinates corresponding to each joint. Let us define p j (q) ∈ R 3 and R j (q) ∈ SO(3) as the position and orientation of link j. The IK problem involves finding coordinates q that archive the target position and orientation of link j. Let us consider the following error where, p d j ∈ R 3 and R d j ∈ SO(3) represent the target position and orientation of link j, and α(R) ∈ R 3 indicates the conversion from a rotation matrix to an angle-axis vector.
In a typical IK problem, to make the error in (1) zero, the nonlinear equation (i.e., e j (q) = 0) is solved with respect to q. When solving the equation, the following linear equations about the translational and angular velocitiesṗ j and ω j ∈ R 3 can be utilized such that where J j is the basic Jacobian matrix [19]. The nonlinear equation cannot always be solved in general, and there exist several problems related to numerical stability. Owing to those issues, the problem of solving e j (q) = 0 is often replaced with the optimization problem of minimizing the error in (1): where Σ L,j ∈ R 6×6 is the weight matrix for the error of link j and semi-positive definite. If there is no constraint related to link j, matrix Σ L,j can be set to zero. The errors and weight matrices of all links are concatenated as the single vector/matrix forms e L and Σ L , respectively: where, notation diag(· · · ) indicates the block diagonal matrix constructed by the given matrix entries. The optimization problem (3) can be solved using the following iterative method: where,q (k) is treated as the direction vector used in the k-th iteration, and α ∈ R is the step size used in each update. The gradient vector, g ∈ R N J , of the cost function can be computed as Matrix H (k) can be determined using various methods, such as the steepest gradient method, conjugate gradient method, and quasi-Newton method. Most methods can guarantee global convergence when combined with a line search algorithm that appropriately finds α. The convergence speed and computational time of each iteration depend on the DoF of the system as well as the number of constraints of IK.
Here, let us utilize the Levenberg-Marquardt (LM) method [25]. The update rule of H can be written as This form of the inverse matrix is also known as the singular robust inverse or damping inverse [4], [5]. There are several design ways about the damping factor Σ Q ∈ R N ×N . Let us consider the following positive definite matrix: where λ Q,i,j is a positive scalar. In general, instead of computing B −1 , the following linear equations are solved directly Although the LM method is effective in terms of convergence speed and numerical stability, the computational complexity of each iteration can be O(N 3 ), resulting in the computational cost, especially in the case of the IK problem of the large-DoF system. This study was aimed at speeding up the computation of solving the linear equation, (14), by utilizing the efficient computation algorithm of robotics, to maximize the characteristics of the LM method and realize a robust and fast IK computation.

B. Equivalent Problem Using the Virtual Dynamical System
Translational velocityṗ j and angular velocity ω j of link j are represented in the Cartesian coordinates (or the task space). Subsequently, let us consider spatial velocity ν j which is represented in the local link coordinates such that where A j is the spatial transformation matrix defined as Let us assume the following virtual dynamical system.
r Let Σ L,l × R 6×6 be the inertia tensor of each link l of the virtual system defined as (17) r Each joint has the virtual actuator with inertia Σ Q ,j . r There exists no gravity in the system.
Note that (17) establishes the mapping between the weighting matrix for IK, denoted as Σ L,l , in the task space, and the virtual inertia matrix, denoted as Σ L,l , in the local link coordinates of the dynamical system.
The Lagrangian of the above system can be obtained as As the first term in (18) is represented in the local link coordinate frame, let (18) be transformed by the mapping in (17) with (15) as follows: The generalized momentum of this virtual system can be written as The inertia matrix in (22) is the same form as updating matrix B in (11). Therefore, the problem of solving (14) can be replaced by the problem of solving (22), assuming the generalized momentum such that ρ J = g. The aforementioned replacement, substituting LM-based IK problem with the pseudo FD problem, is the fundamental concept of LM-PFD.
When solving (22), the fast algorithms of the FD computation [28], [29], [30] can be used. Especially, in the case of open kinematic chains, the Articulated-Body Algorithm (ABA) [28] is very efficient and can solve the problem with computational complexity O(N ). The actual formulations of solving (22) by ABA will be shown in Appendix A.

C. Summary
In the iterative process of the IK computation, (14) can be replaced with the problem of solving the generalized momentum equations in (22) with respect to the generalized velocity,q. The processes of iteration k in the proposed IK are summarized as follows.
1) Compute gradient vector g (k) of the cost function in (3), and assume the generalized momentum such that ρ J = g (k) . Though the gradient can be obtained directly from (8), it can be computed by utilizing the fast recursive algorithms [31], as shown in [27]. 2) Update inertia tensors Σ L,l of the virtual system according to (17). 3) Solve (22) with respect toq (k) by utilizing the FD algorithms [28] without solving the linear equations directly. 4) Update q (k) according to (6). The whole procedure of IK computation is the same as that of the standard iterative methods.
There are several notes on the proposed method. Firstly, weighting matrix Σ L,l in (3) is expressed in the Cartesian space, whereas virtual inertia tensor Σ L,l is represented in the local link coordinates. As a result of the mapping from the Cartesian space to the local link coordinates, Σ L,l undergoes updates at each iteration based on (17). It should be noted that (22) remains valid only during the corresponding iteration. Hence, in contrast to motion generation problems such as trajectory planning or motion optimization [32], [33], the trajectory resulting from the temporal integration of the differential system, as denoted by (22), lacks mechanical significance.
Secondly, it is assumed that virtual inertia tensor Σ L,l (or weighting matrix Σ L,l ) is not strictly positive-definite but rather positive semi-definite. For example, in the case of IK, where solely the desired end-effector posture is provided, Σ L,l = O holds except for the end-effector link. Even in such cases, (22) remains solvable if matrix Σ Q is positive-definite. Additional details can be found in Appendix A.
Finally, in the usual IK problems, the computational time depends on the number of constraints (i.e., e j (q) = 0). Owing to the introduction of weight matrices Σ L,l for all links, the computational complexity of solving (22) can be O(N ), and this does not change without depending on the number of constraints, except for the process of updating the virtual inertia tensors. The proposed method is effective especially when the number of constraints is large.

III. IMPLEMENTATION METHOD FOR CLOSE KINEMATIC CHAINS
In the FD algorithms [29], [30] for closed kinematic chains, the equations of motion are represented in the link coordinate systems instead of the joint coordinate systems, while the constraints of joint motion are explicitly introduced as the equality constraints.
Let us consider joint j, which connects link p j and c j , and let ν j p j ,c j ∈ R 6 be the relative spatial velocity of links c j from p j such that where matrix S j,k is a spatial coordinate transformation matrix that maps the spatial velocity from coordinate k to coordinate j.
The joint constraints about the relative velocity can be written as where matrix C j ∈ R 6−n j ×6 consists of orthogonal bases and extracts the constraint components from the spatial velocity. For ease of explanation, let us transform (24) with respect to (23) into the following form where S j has two nonzero block matrices corresponding link p j and c j as follows The following relationship between joint velocityq j and relative velocity ν j p j ,c j also holdṡ where matrix K j ∈ R n j ×6 consists of the complementary orthogonal bases of C j and extracts the joint velocity components from the spatial velocity. The complementary relationships of K j and C j are denoted as where I is an identify matrix. Now, let us consider the augmented Lagrangian according to the equality constraint in (25) as follows where λ j ∈ R n j is the Lagrange multiplier corresponding to the j-th joint constraint. From (12), (18), (19), (27), and (30), L 1 can be transformed to Therefore, the equations about the link-space momentum, ρ L ∈ R 6N L , and the Lagrange multipliers take the following form where Λ ∈ R 6N L ×6N L is the inertia matrix corresponding to the link equations as follows The relationships between the variables represented in the link space and those in the joint space are summarized aṡ The flow of IK does not considerably differ from that introduced in Section II. In Step 3), the FD algorithms for closed kinematic chains [29], [30] are applied to solve (37), while utilizing (39) and (40) in the process.
As the matrices Σ L , Σ Q , K, C, and S are sparse, (37) can also be directly solved by utilizing a sparse linear solver. When solving sparse linear equations, the ordering process is important for permuting the rows and columns of the sparse matrix before applying matrix factorization. The FD algorithms in robotics are special implementations of the ordering process utilizing the joint connectivity of the kinematic chain. Nevertheless, general ordering algorithms, such as the approximate minimum degree ordering [34], can also be used, and they are easily available in standard sparse solvers.

IV. IMPLEMENTATION METHOD FOR WIRE-DRIVEN KINEMATIC CHAINS
This section presents the further implementation method for a wire-driven kinematic chain. First, let us formulate the IK problem in the case of a wire-driven system.

A. IK of Wire-Driven Kinematic Chain
Let N W be the total number of wires in the link system. The w-th wire (1 ≤ w ≤ N W ) passes through several via-points, where each via-point is located on a link, as illustrated in Fig. 1. Let n w (≥ 2) is the number of via-points of the w-th wire, b(w, k) indicates the index of the link where the k-th via-point of the w-th wire is located, and r w,k be the global position of k-th via-point of w-th wire.
Length l w ∈ R of the wire is computed as where, r w,k indicates the relative position of the k-th via-point of the w-th wire with respect to the origin of the coordinate system of link r w,k . Let V(i, w) be the index set of w-th wire's via-points attached on i-th link of the kinematic chain. Velocityl w of the wire length can be computed as [10]l indicates the set of via-points of w-th wire attached on i-th link excluding the first via-point, and E(i, w) {k ∈ V(i, w) | k = n w } means the set excluding the last (or n w -th) via-point. The IK of the wire-driven multibody system can be formulated as follows where Σ W ∈ R N W ×N W and e W ∈ R N W are the concatenated form defined as Each wire has a constraint such that l w ≤ l d w , where l d w ∈ R is the natural length of the wire. The error about the inequality constraint of w-th wire is represented as d w ∈ R, and the corresponding weighting factor is σ w ∈ R.
Then, the gradient of cost functionf can be computed as follows When solving the optimization problem in (48), the updating matrix, B, used in the LM method is written as In the IK of the wire-driven system, B and g can be obtained from (52) and (54) to construct the linear (14).

B. Virtual Dynamics Problem Equivalent of a Wired System
Let us assume the following Lagrangian for the wired kinematic chain where the first term is equivalent to (18) and the second is related to the wire kinematics of (43). The generalized momentum of the above-mentioned virtual system can be written as where H is the inertia matrix of the virtual dynamical system and is defined as From (54) and (57), inertia matrix H can be of the same form as the updating matrix, B, used in the LM method. Hence, the problem of solving (14) can be replaced by that of solving (56) when assuming the generalized momentum such that ρ J = g.
In the next step, let us represent the formulation of (56) with respect to the link coordinates, being similar to the discussion in Section III.
From (15) and (43), wire velocityl w can be written by using spatial velocities ν as followṡ where L w ∈ R N W ×6N L can be denoted as Similar to the discussion of (18) and (30), the augmented Lagrangian corresponding to (55) under the joint constraints can be derived from (58) as followŝ Authorized licensed use limited to the terms of the applicable license agreement with IEEE. Restrictions apply.
The equations about the link-space momentum and Lagrange multipliers are denoted as Therefore, the problem of solving (14) under (52) and (54) can be replaced with that of solving (62).

C. Approximation for Faster Computation
Since (62) for closed kinematic chains includes explicit formulas related to joint constraints, additional computations are required to solve the Lagrange multipliers by solving the entire set of complex equations, unlike in the case of (22) for open kinematic chains. On the other hand, as the LM method originally approximates the inverse of the Hessian matrix by using the damping inverse of the Jacobian matrix in its iterative computations, (62) does not need to be solved in an exact manner. In order to reduce the computation time, let us approximate (62) by adding a new damping factor, μ(> 0) ∈ R, such that The solution of ν for (64) can be written as In (64), ν can be directly obtained without computing Lagrange multipliers λ. The sparse structure ofΛ depends on the connectivity of not only the joints but also wires. Therefore, in the case of wire-driven systems, the general FD algorithms cannot be applied. In this study, the approximate minimum degree method [34] is adopted for matrix ordering. Note that during the iterative computation, the structure ofΛ does not change. Hence, the ordering process and the memory allocation for the factorization process are required to be computed only once before computing IK.

A. Computational Evaluation Using Manipulators
The proposed method and other existing algorithms were tested on the IK of the manipulator consisting of spherical joints to analyze their performance with respect to the number of DoF. In the numerical evaluation, the two manipulators with different DoF are used: N = 60, 600. Similar to the evaluation in [27], In addition, the target position of each link is set at The following methods were evaluated: r (LM): the LM method with the damping design proposed by Sugihara [21].
r (LM-PFD(ABA)): the LM method [21] solving the pseudo FD problem in (22) by the ABA method [28] r (LM-PFD(AVD)): the LM method [21] solving the pseudo FD problem according to the approximated virtual dynamics (AVD) denoted in (64) r (QN): the quasi-Newton method (BFGS formula) with the gradient computation method [27] r (CG): the conjugate gradient method (DY formula [35]) with the gradient computation method [27]. In the line search process, step size α was computed according to the Armijo condition [36]. The gradient of the cost function was computed by the decomposed gradient computation described in [27], which particularly demonstrates its effectiveness when applied to the IK for the system with large DoFs. In this letter, the Sugihara's method [21] is utilized for all IK methods based on the LM methods (LM, LM-PFDs). According to the method, the damping factor is designed such that where f k represents the value of the cost function in (48) at k-th iteration, and d is the double-precision machine epsilon. In the case of LM-PFD(AVD), damping factor μ is designed such that: μ = N √ d . The iterative computation is repeated until the error cost function, f , is less than 1.0 × 10 −6 The method was implemented on a mobile workstation with an Intel(R) Core(TM) i9-12900H.
The comparison of each optimization method is summarized in Table I Table I. LM-PFD(ABA) solves the exact same IK problem as the standard LM method. In contrast, LM-PFD(AVD) approximates  the IK problem by introducing μ. As shown in Table I, the approximation results in a better computational performance compared to the other methods.

B. Musculoskeletal IK
Next, the computational performance of the proposed method and that of the other methods are compared based on the IK computation of the human musculoskeletal model used in [10]. In this musculoskeletal model, the skeletal system is represented as a multibody kinematic chain, and the muscles and tendons are approximated as massless wires. Note that the total number of wires is larger than the number of anatomically classified muscles/tendons because, for example, muscles with a large volume are represented by multiple wires. In addition, virtual links (6 DoF) are located so as to represent the branches of muscles or tendons. Therefore, the total DoF of the model exceeds that of the whole joints. In this study, the model with 155 DOFs and 338 wires was evaluated.
To confirm the IK tracking performance of IK in the case of quick movements, the jumping motion of a human subject was measured by a commercial optical motion capture system (Motion Analysis Corp.). The system measures the position of 34 reflective markers attached to the anatomical points of the human body at 200 Hz. Our study protocol was approved by the local institutional review board and conformed to the guidelines outlined in the Declaration of Helsinki (1983). From the measured trajectories of the markers, the musculoskeletal IK was computed by minimizing the sum of the error between the measured and estimated marker position. The following IK algorithms were evaluated: LM, LM-PFD(AVD), QN, and CG. As the original ABA algorithm work only for open kinematic chains, LM-PFD(ABA) cannot be utilized for the IK of the musculoskeletal model and has been excluded from the evaluation. The snapshots of the reconstructed jumping motion of the musculoskeletal model are shown in Fig. 2.
The iterative computation of each IK algorithm stops within 5 ms according to the frame rate of the motion capture system.
As the tracking errors cannot be zero owing to measurement noise and modeling errors, the sufficiently convergent trajectory from IK with multiple repetitions of the iterative computations was used as the baseline to be compared. Let p M,i andp M,i be i(1 ≤ i ≤ 34)-th marker's position reconstructed from each IK result and the baseline trajectory, respectively. The maximum marker tracking error, max({||p M,i − p M,i ||}), of each IK algorithm from the baseline is shown in Fig. 3. The LM-PFD(AVD) method showed the best tracking performance, and there is little difference from the convergent trajectory. The tracking error increases when the subject is in fast motion. The maximum error of the proposed method is at most 3 mm except at the beginning when the IK tracking starts. This error is in the same order as the spatial resolution of the motion capture system. The maximum error of any other method is over 2 cm, which is not in the acceptable range.

VI. CONCLUSION
This letter presented LM-PFD, a fast IK method for large-DoF multibody systems. In the method, the IK problem is replaced with the pseudo FD problem by assuming the virtual system, the inertial properties of which are derived from the weighting or damping factors in the LM method. The method can significantly accelerate the computational speed of the LM method by utilizing efficient FD algorithms. In this letter, the basic implementation of LM-PFD utilizing ABA method [29] was initially introduced for open kinematic chains. Furthermore, the enhanced method for closed kinematic chains, particularly for wire-driven systems, was also proposed.
The proposed method efficiently addresses the issue of the LM method about the computational speed in the case of large-DoF systems, while maximizing the strengths of the LM method in terms of convergence speed throughout iterations and computational robustness. These characteristics have been proved by comparing the proposed method and other existing algorithms in the numerical evaluation. The method was also tested on solving IK of the human musculoskeletal model [10]. The computational time when using the model with approximately 150 DoF and 300 wires was within 5 ms.
In our future work, the computational acceleration by introducing the pseudo FD problem will be also applied to other kinematics or dynamics problems such as muscle tension estimation [10] or motion optimization [32], [33].
APPENDIX A ARTICULATED-BODY ALGORITHM [28] This appendix section introduces the actual formulations of solving (22) by the articulated-body algorithm [28].
Given Σ L,i , Σ Q ,j and g i , the articulated-body inertia and momentum, I A i and p A i , can be computed recursively from the child-side of the kinematic chain as follows: where C(i) means the children of link i, and