Exploring Energy in the Direct Correction Method for Correcting Geometric Constraint Violations

Abstract

The direct correction method is widely used for eliminating geometric constraint violations. This method involves iteratively adjusting the generalized coordinates, which are assumed to be consistent and remain so during the velocity-level corrections. However, the corrected generalized coordinates cause a significant effect on the velocity constraint violations. In this paper, simultaneously correcting both the generalized coordinates and velocities is proposed. A semi-analytic approach to solve the Jacobian matrix, which is used to correct the generalized coordinates and velocities, was employed. Further, the position level, velocity level, and energy constraint equations were corrected simultaneously to ensure that the corrected generalized coordinates and velocities complied with the dynamic equations. The corresponding semi-analytic Jacobian matrix was derived to solve the constraint equations. The methods were demonstrated to be effective using examples, with the simultaneous correction of position-level and velocity-level constraints showing the best results when combined with the energy correction.

1. Introduction

Multibody system dynamics is an important area of study that focuses on modeling the behavior of large and complex mechanical systems precisely and efficiently [1,2,3,4,5,6,7,8]. Much commercial software in dynamics, such as Simpack [9,10], utilizes specific approaches in the underlying layer, using hinge coordinates as generalized coordinates and avoiding global dynamics equations to improve computational efficiency. The approaches include the reduced multibody system transfer matrix method (RMSTMM) [11] and recursive methods [12,13]. They establish a set of ordinary differential equations for an open-loop tree system [14,15]. In contrast, a set of differential algebraic equations (DAEs) is generated to describe the behavior of the closed-loop system [3,7]. The solution to the DAEs is continually improved to ensure computational efficiency, accuracy, and robustness.

There are three main categories for solving DAEs in multibody system dynamics: constraint stabilization approaches, coordinate partitioning methods, and direct correction formulations [16,17]. Constraint stabilization approaches commonly employ the Baumgarte method, which incorporates control feedback theory [18]. However, this approach has certain limitations in terms of stabilization coefficients [19,20,21]. Coordinate partitioning methods [22,23,24] preserve the accuracy of the original dynamic equations but have high computational costs and poor numerical efficiency [25]. In contrast, direct correction formulations offer improved accuracy over the Baumgarte method, while also showing better computational efficiency compared to coordinate partitioning methods [26,27,28].

Direct correction formulations are widely used in multibody system dynamics during the numerical solution of the DAEs [29]. Various techniques of the direct correction have been developed [16,30,31,32,33]. Yoon et al. [34] established a direct correction method to directly correct the values of state variables, resulting in a better fit with the constraint equations. It is worth mentioning, however, that this method is limited to focusing on only position-level constraints. On the other hand, Yu and Chen [28] resolved violation errors by implementing constraints at both the position and velocity levels. The effectiveness of this approach was demonstrated through a simple case, which was compared to the standard formulation and the Baumgarte method. The coordinate projection method [35] combines the direct correction with the constraint manifolds to ensure that the constraints manifold at the position and velocity levels are met in the numerical solution of the DAE by directly adding constraint equations and performing corrections in the orthogonal-to-constraint directions, as illustrated in Figure 1. Gradually, other factors are beginning to be considered. The effect of element inertia is considered by incorporating the mass of an element as a weighting term in mathematical calculations. This results in a more accurate solution as the mass of an element increases, as there is less movement within the constraints [36,37,38,39].

Furthermore, the corrected dynamics are desired to be as close as possible to the original dynamic equations. Violation problems can result in errors in estimating a system’s energy due to constraints changes [16,29]. To address this challenge, extensive research has recently been conducted on system-energy-preserving schemes. Yoon [34] introduced a geometric approach for eliminating constraint violations through a gradient-based procedure. The correction of generalized coordinates was based on the geometric constraint equations, while the correction of generalized velocities was based on the energy constraint. Blajer [38] employed energy constraints to regulate velocity exclusively based on the Lagrangian multiplier methods. Yoon et al. [40] explored different combinations of energy constraint control with other constraint stabilization methods, including the Baumgarte method and the new constraint violation stabilization using gradient feedback. A new approach for implementing constraint controls based on the geometric interpretation of the phase space has been developed using the Euler method. However, these methods have only undergone testing in basic scenarios. Zhang et al. [32] introduced the energy norm method to suppress the constraint violation in three-dimensional multibody systems with a singular mass matrix. Orden et al. proposed using a velocity projection based on the Lagrangian method to eliminate energy constraint violations [41,42]. Changes in both generalized coordinates and velocities can impact the energy of the system. Further research is necessary to fully understand the impact of these changes on the constraints and energy of the system.

The dynamics equation of a multibody system, using RMSTMM as a case, is briefly described in Section 2. In order to address the constraint violation problem encountered when handling closed-loop systems, Section 3 introduces the direct correction method using a two-step approach. First, correcting the generalized coordinates using the constraint equations on the position level through an iterative solution, and then, correcting the generalized velocities using the constraint equations at the velocity level. However, the corrected generalized coordinates directly affect the velocity constraint equations. An attempt is made to directly correct the generalized coordinates and velocities by combining the constraint equations at both the position and velocity levels in Section 4. However, this approach does not take into account the effect of corrected generalized coordinates and velocities on the dynamic equations. Energy constraint equations are added and combined to solve this with the constraint equations on the position and velocity levels to correct the generalized coordinates and velocities in Section 5. The article also notes that the semi-analytical forms of the Jacobian matrices used to correct the generalized coordinates, velocities, and energy are derived in the corresponding section. The effectiveness of this approach is verified through several examples presented in the final section.

2. Constrained Dynamic Systems

Over the years, the solutions for dynamics problems have been diversified. The RMSTMM is widely used by avoiding global dynamics equations with a system inertia matrix. It derives acceleration equations in a recursive manner and can be calculated efficiently [3,11].

The essence of the RMSTMM involves discretizing the system into different elements, such as body and joint elements, and using linear relationships among kinematic and kinetic quantities acting on each element. The method utilizes a general equation, known as the reduced transfer equation, to transfer the dynamical properties of each element and establish the dynamics of the entire system in a recursive manner. The form of reduce transfer equation is as follows:

$$z_{a,\mathrm{p}}=S_{\mathrm{p}}{z}_{b,\mathrm{p}}+e_{\mathrm{p}}+{\displaystyle {\sum }_j{\chi }_{\mathrm{p},{\mathrm{L}}_j}{z}_{a,\mathrm{C},{\mathrm{L}}_j}},$$

(1)

where

$$z_{a,\mathrm{p}}={\left[\begin{array}{cc}{m}^{\mathrm{T}}& f^{\mathrm{T}}\end{array}\right]}_{\mathrm{p}}^{\mathrm{T}}, z_{b,\mathrm{p}}={\left[\begin{array}{cc}{\ddot{r}}^{\mathrm{T}}& {\dot{\Omega }}^{\mathrm{T}}\end{array}\right]}_{\mathrm{p}}^{\mathrm{T}} .$$

(2)

where $z_{a,\mathrm{p}}$ include internal torque $m$ and internal force $f$ and $z_{b,\mathrm{p}}$ include translational acceleration $\ddot{r}$ and angular acceleration $\dot{\Omega }$. $L_j$ is the jth closed loop. The constraint acting on the jth cut-joint is treated as force $z_{a,\mathrm{C},{\mathrm{L}}_j}$. $S_{\mathrm{p}}$, $e_{\mathrm{p}}$, and ${\chi }_{\mathrm{p},{\mathrm{L}}_j}$ are coefficient matrices of point $\mathrm{P}$.

The RMSTMM is an algorithm, which, given generalized coordinates $q$ and velocities $\dot{q}$ of a mechanical multibody system, computes the generalized accelerations ($\ddot{q}$). The generalized acceleration of a multibody system is obtained using the transfer equation as follows:

(a)

The spanning tree system can be derived by cutting the connecting joint, and the initial values for the reduced transfer matrices are set based on the equations for the cut-joint;

(b)

The reduced transfer matrices for each element can be determined by sweeping through the spanning tree system;

(c)

Force $z_{a,\mathrm{C},{\mathrm{L}}_j}$ is determined by using the supplementary equations for the cut-joint and iteratively sweeping through the spanning tree;

(d)

The intermediate vectors $z_{a,\mathrm{p}}$ and $z_{b,\mathrm{p}}$ can be calculated recursively using the reduced transfer equations in the reverse direction of the transfer path. The generalized acceleration can be obtained as described in reference [43].

Finally, a numerical algorithm for solving ordinary differential equations uses the generalized acceleration obtained to evaluate the generalized coordinates and velocities of the next step. During the solution process, the global dynamics equations are avoided. A fast, accurate solution to the violation problem is paramount, especially in the unknown overall dynamics equations. The direct correction method was favored due to its simplicity, high accuracy, and low computational cost. This method directly supplements the constraint equations and performs corrections, making it an effective solution for addressing violation problems.

3. Direct Elimination of Position-Level and Velocity-Level Geometric Constraint Violations

3.1. Direct Correct Formulations

A general and comprehensive direct correction method [40] accurately solves the constraint divergence problem by eliminating the constraint violation at both the position and velocity levels without increasing the computational effort. During the numerical solution of the dynamic method, the constraint on the acceleration level is a strong constraint that must be met. However, constraints on the position and velocity levels may not be upheld due to integration errors. To control this error, the constraint equations for the position and velocity levels were introduced. The system’s generalized coordinates were corrected firstly using the Newton–Raphson iteration method, denoted as

$$c\left(q\right)+c_q\Delta q =\mathsf{0},$$

(3)

where the constraint equations at the position level of a cut-joint are denoted as $c$, and the rectangular matrix representing the constraint Jacobian is denoted as $c_q$.

The velocity constraint is modified by considering only the effect of the generalized velocities as the generalized coordinates have been corrected. In turn, it is known that the Jacobian matrix $c_q$ can represent the derivative of the velocity constraint equations with respect to generalized velocities ${\dot{c}}_{\dot{q}}$. The Newton–Raphson formula for the constraint equation on the velocity level can be expressed as

$$\begin{array}{c}\mathsf{0}=\dot{c}\left(q,\dot{q}+\Delta \dot{q}\right)\\ \approx \dot{c}\left(q,\dot{q}\right)+{\dot{c}}_{\dot{q}}\Delta \dot{q}\\ =\dot{c}\left(q,\dot{q}\right)+c_q\Delta \dot{q}.\end{array}$$

(4)

The constraint equations were established based on the type of cut-off joint. A pin joint element moving in the plane was used for completeness. The base and moving ends of the cut-off joint were marked as B and M, respectively. The relative displacement of the cut-off pin joint moving in the plane was restricted, and the constraint equation on the position level can be expressed as

$$c=\left[r_{\mathrm{OM}}-r_{\mathrm{OB}}\right]=\mathsf{0},$$

(5)

where $r_{\mathrm{OB}}$ denotes the position of the base marker with respect to the inertial frame, $r_{\mathrm{OM}}$ denotes the position of the moving marker, and subscript $\mathrm{O}$ denotes the inertial coordinate system.

The constraint equation on the velocity level, using the example of the pin cut-off joint, can be established as

$$\dot{c}=\left[{\dot{r}}_{\mathrm{OM}}-{\dot{r}}_{\mathrm{OB}}\right]=0,$$

(6)

where ${\dot{r}}_{\mathrm{OB}}$ denotes the velocity of the base marker with respect to the inertial frame and ${\dot{r}}_{\mathrm{OM}}$ denotes the velocity of the moving marker.

The Jacobian matrices $c_q=\partial c/\partial q$ are typically derived using the direct differential method. However, as a system’s complexity increases, the Jacobian matrix’s complexity also increases, resulting in a significant computational burden. In order to solve this issue, the derivations of the semi-analytic Jacobian matrices are discussed in greater detail below.

3.2. The Jacobian Matrix of the Constraint Equation

The Jacobian matrix is an essential tool used to analyze the dynamics of systems and can be represented as a matrix of partial derivatives of constraints with respect to the system’s generalized coordinates. The system Jacobian matrix, as presented in Equation (3), can be represented as

$$\frac{\partial c}{\partial q}=\left[\begin{array}{ccc}\frac{\partial c}{\partial q_1}& \cdots & \frac{\partial c}{\partial q_n}\end{array}\right].$$

(7)

Assuming that all other generalized coordinates remain constant, the constraint equation with respect to generalized coordinate $q_j$ can be expressed as follows:

$$c\left(q_j\right)+\frac{\partial c}{\partial q_j}\left(q_j\right)\Delta q_j=0.$$

(8)

The sub-matrix $\partial c/\partial q_j$ of the Jacobian matrix can be derived based on the characteristics of a joint element. It can be derived in the semi-analytical form in the following manner.

Let us consider the case of pin joint element j rotated about the z-axis of the body-fixed frame and encountered in the closed loop. The angle of the joint serves as the generalized coordinate, represented by

$$q_j=\left[{\theta }_{z,j}\right].$$

(9)

The root and tip ends of pin joint j are marked as ${\mathrm{R}}_j$ and ${\mathrm{T}}_j$, as shown in Figure 2. If this joint is associated with the B end of cut-joint L, then changes in generalized coordinate $q_j$ of the joint will not affect the displacement of the M end in Equation (5), no matter how it changes. The displacement of the B end, on the other hand, is described by

$$r_{\mathrm{OB}}=r_{{\mathrm{OR}}_j}+r_{{\mathrm{R}}_j{\mathrm{T}}_j}+r_{{\mathrm{T}}_j\mathrm{B}}=r_{{\mathrm{OR}}_j}+A_{{\mathrm{OR}}_j}{l}_{{\mathrm{R}}_j{\mathrm{T}}_j}+A_{{\mathrm{OR}}_j}{A}_{{\mathrm{R}}_j{\mathrm{T}}_j}{A}_{{\mathrm{OT}}_j}^{\mathrm{T}}{r}_{{\mathrm{T}}_j\mathrm{B}},$$

(10)

where subscripts ${\mathrm{R}}_j$ and ${\mathrm{T}}_j$ denote the body-fixed coordinate system with points ${\mathrm{R}}_j$ and ${\mathrm{T}}_j$, respectively; $r_{\mathrm{OP}}$ denotes the position of P point with respect to the inertial frame; $l_{{\mathrm{R}}_j{\mathrm{T}}_j}$ describes the position vector from ${\mathrm{R}}_j$ point to ${\mathrm{T}}_j$ point in the body-fixed coordinate system with the ${\mathrm{R}}_j$ point; and cosine matrix $A_{\mathrm{OP}}$ denotes the coordinate transformation from the body-fixed coordinate system as the origin with the $\mathrm{P}$ point to the inertial frame, as noted in

$$A_{\mathrm{OP}}=\left[\begin{array}{cc}\cos {\theta }_{\mathrm{OP}}& -\sin {\theta }_{\mathrm{OP}}\\ \sin {\theta }_{\mathrm{OP}}& \cos {\theta }_{\mathrm{OP}}\end{array}\right].$$

(11)

The kinematic quantities were transferred along with the elements recursively using the RMSTMM. As a result, the quantities at the root end of the joint element passed from the inboard element would not change, irrespective of this generalized coordinate. Since the pin joint can only rotate around the z-axis, $l_{{\mathrm{R}}_j{\mathrm{T}}_j}$ will be zero. Furthermore, displacement $A_{{\mathrm{OT}}_j}^{\mathrm{T}}{r}_{{\mathrm{T}}_j\mathrm{B}}$ described in the coordinate system fixed on the tip end of the joint will also be independent of the generalized coordinate. The partial derivative of Equation (10) with respect to the generalized coordinate of the joint element can be represented as

$$\begin{array}{l}\frac{\partial }{\partial q_j}{r}_{\mathrm{OB}}=\frac{\partial }{\partial q_j}(r_{{\mathrm{OR}}_j}+A_{{\mathrm{OR}}_j}{l}_{{\mathrm{R}}_j{\mathrm{T}}_j}+A_{{\mathrm{OR}}_j}{A}_{{\mathrm{R}}_j{\mathrm{T}}_j}{A}_{{\mathrm{OT}}_j}^{\mathrm{T}}{r}_{{\mathrm{T}}_j\mathrm{B}})\\ =A_{{\mathrm{OR}}_j}\frac{\partial A_{{\mathrm{R}}_j{\mathrm{T}}_j}}{\partial q_j}{A}_{{\mathrm{OT}}_j}^{\mathrm{T}}{r}_{{\mathrm{T}}_j\mathrm{B}}\\ =A_{{\mathrm{OR}}_j}D{A}_{{\mathrm{R}}_j{\mathrm{T}}_j}{A}_{{\mathrm{OT}}_j}^{\mathrm{T}}{r}_{{\mathrm{T}}_j\mathrm{B}}\\ =D{r}_{{\mathrm{T}}_j\mathrm{B}},\end{array}$$

(12)

where a relationship exists for joint j that can be represented as

$$\partial A_{{\mathrm{R}}_j{\mathrm{T}}_j}/\partial q_j=D{A}_{{\mathrm{R}}_j{\mathrm{T}}_j},$$

(13)

$$D=\left[\begin{array}{cc}0& \hfill -1\\ 1& \hfill 0\end{array}\right].$$

(14)

By substituting Equation (12) into the partial derivative of Equation (5), the Jacobian submatrix can be obtained as

$$\frac{\partial c}{\partial q_j}=-\frac{\partial r_{\mathrm{OB}}}{\partial q_j}=\left[-D{r}_{{\mathrm{T}}_j\mathrm{B}}\right].$$

(15)

If joint j is associated with the M end of cut-joint L, the same applies to $\partial r_{\mathrm{OM}}/\partial q_j$, denoted as

$$\begin{array}{l}\frac{\partial }{\partial q_j}{r}_{\mathrm{OM}}=\frac{\partial }{\partial q_j}(r_{{\mathrm{OR}}_j}+A_{{\mathrm{OR}}_j}{l}_{{\mathrm{R}}_j{\mathrm{T}}_j}+A_{{\mathrm{OR}}_j}{A}_{{\mathrm{R}}_j{\mathrm{T}}_j}{A}_{{\mathrm{OT}}_j}^{\mathrm{T}}{r}_{{\mathrm{T}}_j\mathrm{M}})\\ =A_{{\mathrm{OR}}_j}\frac{\partial A_{{\mathrm{R}}_j{\mathrm{T}}_j}}{\partial q_j}{A}_{{\mathrm{OT}}_j}^{\mathrm{T}}{r}_{{\mathrm{T}}_j\mathrm{M}}\\ =D{r}_{{\mathrm{T}}_j\mathrm{M}}.\end{array}$$

(16)

By substituting Equation (16) into the partial derivative of Equation (5), the Jacobian submatrix can be derived as

$$\frac{\partial c}{\partial q_j}=\frac{\partial r_{\mathrm{OM}}}{\partial q_j}=\left[D{r}_{{\mathrm{T}}_j\mathrm{M}}\right].$$

(17)

4. Simultaneous Eliminations of Position-Level and Velocity-Level Geometric Constraint Violations

The general approach used is to adjust the generalized coordinates and velocities in succession. The velocity constraint equations, however, are influenced not only by the generalized velocities but also by the corrected generalized coordinates. The correction of generalized coordinates will cause the velocity constraint equations to be further violated. Here, the generalized coordinates and velocities were corrected together, which is studied in this section.

4.1. Simultaneous Formulation for Correcting Position-Level and Velocity-Level Constraints

The general direct correction method maintains a clear linear relationship between the constraints on the velocity level and the generalized velocities. Nevertheless, the generalized coordinates and velocities can affect the velocity constraints, resulting in a more complex relationship that is no longer purely linear. In such a case, the constraint equation on the velocity level will be expressed as

$$\dot{c}\left(q,\dot{q}\right)=0.$$

(18)

The equation representing the elimination of violation on the velocity level through adjusting the generalized coordinates and velocities is expressed as

$$\dot{c}\left(q,\dot{q}\right)+\frac{\partial \dot{c}}{\partial q}\Delta q+\frac{\partial \dot{c}}{\partial \dot{q}}\Delta \dot{q}=0.$$

(19)

The constraint equation $c_{\mathrm{s}}$ during simultaneous correction changes to

$$c_{\mathrm{s}}=\left[\begin{array}{c}c\left(q\right)\\ \dot{c}\left(q,\dot{q}\right)\end{array}\right]=0.$$

(20)

The Newton–Raphson iteration method is used to correct the generalized coordinates and velocities simultaneously, and it is represented as follows:

$$\left[\begin{array}{c}c\\ \dot{c}\end{array}\right]+\left[\begin{array}{cc}{c}_q& O\\ {\dot{c}}_q& {\dot{c}}_{\dot{q}}\end{array}\right]\left[\begin{array}{c}\Delta q\\ \Delta \dot{q}\end{array}\right]=0.$$

(21)

The Jacobian matrix is essential in understanding how changes in the system’s variables affect its constraints. At this stage, updating the Jacobian matrix by calculating the partial derivatives of the constraint equations on the velocity level with respect to the generalized coordinates and velocities is necessary.

4.2. The Jacobian Matrix of the Constraint Equation

It is known that Jacobian matrix $c_q$ to correct the constraints at the position level is equal to Jacobian matrix ${\dot{c}}_{\dot{q}}$, then the one to be solved is ${\dot{c}}_q$. The Jacobian matrix ${\dot{c}}_q$ is solved similarly to the Jacobian matrix on the position level $c_q$. When pin joint j is associated with the B end of a cut-joint, the following steps should be taken. The generalized coordinate of the joint is denoted as Equation (9), and it will not affect the velocity ${\dot{r}}_{\mathrm{OM}}$ in Equation (6). The velocity ${\dot{r}}_{\mathrm{OB}}$, on the other hand, is described by taking the derivative of Equation (10) with respect to time, read as

$$\begin{array}{l}{\dot{r}}_{\mathrm{OB}}={\dot{r}}_{{\mathrm{OR}}_j}+{\dot{r}}_{{\mathrm{R}}_j{\mathrm{T}}_j}+{\dot{r}}_{{\mathrm{T}}_j\mathrm{B}}={\dot{r}}_{{\mathrm{OR}}_j}+(D{A}_{{\mathrm{OR}}_j}{\dot{\theta }}_{{\mathrm{OR}}_j}{l}_{{\mathrm{R}}_j{\mathrm{T}}_j}+A_{{\mathrm{OR}}_j}{\dot{l}}_{{\mathrm{R}}_j{\mathrm{T}}_j})\\ +(D{A}_{{\mathrm{OR}}_j}{\dot{\theta }}_{{\mathrm{OR}}_j}{A}_{{\mathrm{R}}_j{\mathrm{T}}_j}{l}_{{\mathrm{T}}_j\mathrm{B}}+A_{{\mathrm{OR}}_j}D{A}_{{\mathrm{R}}_j{\mathrm{T}}_j}{\dot{\theta }}_{{\mathrm{R}}_j{\mathrm{T}}_j}{l}_{{\mathrm{T}}_j\mathrm{B}}+A_{{\mathrm{OR}}_j}{A}_{{\mathrm{R}}_j{\mathrm{T}}_j}{\dot{l}}_{{\mathrm{T}}_j\mathrm{B}}),\end{array}$$

(22)

where the first-time derivatives of the directional cosine matrix $A_{{\mathrm{OR}}_j}$ can be expressed as

$${\dot{A}}_{{\mathrm{OR}}_j}=\left[\begin{array}{cc}-\sin \left({\theta }_{{\mathrm{OR}}_j}\right){\dot{\theta }}_{{\mathrm{OR}}_j}& -\cos \left({\theta }_{{\mathrm{OR}}_j}\right){\dot{\theta }}_{{\mathrm{OR}}_j}\\ \cos \left({\theta }_{{\mathrm{OR}}_j}\right){\dot{\theta }}_{{\mathrm{OR}}_j}& -\sin \left({\theta }_{{\mathrm{OR}}_j}\right){\dot{\theta }}_{{\mathrm{OR}}_j}\end{array}\right]=\left[\begin{array}{cc}0& \hfill -1\\ 1& \hfill 0\end{array}\right]\left[\begin{array}{cc}\cos {\theta }_{{\mathrm{OR}}_j}& -\sin {\theta }_{{\mathrm{OR}}_j}\\ \sin {\theta }_{{\mathrm{OR}}_j}& \cos {\theta }_{{\mathrm{OR}}_j}\end{array}\right]{\dot{\theta }}_{{\mathrm{OR}}_j}=:D{A}_{{\mathrm{OR}}_j}{\dot{\theta }}_{{\mathrm{OR}}_j}.$$

(23)

The kinematic quantities $A_{{\mathrm{OR}}_j}$, ${\dot{r}}_{{\mathrm{OR}}_j}$, and ${\dot{\theta }}_{{\mathrm{OR}}_j}$ passed from the inboard element through the kinematics analysis remain constant, irrespective of the generalized coordinate. Furthermore, $l_{{\mathrm{T}}_j\mathrm{B}}$ is the projection of displacement $r_{{\mathrm{T}}_j\mathrm{B}}$ onto the $T_j$-fixed reference frame, which is unaffected by the generalized coordinate. As the pin joint can only rotate around the z-axis, ${\dot{l}}_{{\mathrm{R}}_j{\mathrm{T}}_j}$ will be zero. Only $A_{{\mathrm{R}}_j{\mathrm{T}}_j}$ is affected by the generalized coordinate in Equation (22). The partial derivative with respect to the generalized coordinate of the equation above, based on Equation (13), yields

$$\begin{array}{l}\frac{\partial {\dot{r}}_{\mathrm{OB}}}{\partial q_j}=D{A}_{{\mathrm{OR}}_j}{\dot{\theta }}_{{\mathrm{OR}}_j}\frac{\partial A_{{\mathrm{R}}_j{\mathrm{T}}_j}}{\partial q_j}{l}_{{\mathrm{T}}_j\mathrm{B}}+A_{{\mathrm{OR}}_j}D\frac{\partial A_{{\mathrm{R}}_j{\mathrm{T}}_j}}{\partial q_j}{\dot{\theta }}_{{\mathrm{R}}_j{\mathrm{T}}_j}{l}_{{\mathrm{T}}_j\mathrm{B}}+A_{{\mathrm{OR}}_j}\frac{\partial A_{{\mathrm{R}}_j{\mathrm{T}}_j}}{\partial q_j}{\dot{l}}_{{\mathrm{T}}_j\mathrm{B}}\\ =D{A}_{{\mathrm{OR}}_j}{\dot{\theta }}_{{\mathrm{OR}}_j}{A}_{{\mathrm{R}}_j{\mathrm{T}}_j}D{l}_{{\mathrm{T}}_j\mathrm{B}}+A_{{\mathrm{OR}}_j}D{A}_{{\mathrm{R}}_j{\mathrm{T}}_j}D{\dot{\theta }}_{{\mathrm{R}}_j{\mathrm{T}}_j}{l}_{{\mathrm{T}}_j\mathrm{B}}+A_{{\mathrm{OR}}_j}{A}_{{\mathrm{R}}_j{\mathrm{T}}_j}D{\dot{l}}_{{\mathrm{T}}_j\mathrm{B}}\\ =-A_{{\mathrm{OT}}_j}{l}_{{\mathrm{T}}_j\mathrm{B}}{\dot{\theta }}_{{\mathrm{OR}}_j}-A_{{\mathrm{OT}}_j}{l}_{{\mathrm{T}}_j\mathrm{B}}{\dot{\theta }}_{{\mathrm{R}}_j{\mathrm{T}}_j}+A_{{\mathrm{OT}}_j}D{\dot{l}}_{{\mathrm{T}}_j\mathrm{B}}.\end{array}$$

(24)

By combining Equation (24) with the partial derivative of Equation (6) for generalized coordinate $q_j$, one obtains

$$\frac{\partial \dot{c}}{\partial q_j}=-\frac{\partial {\dot{r}}_{\mathrm{OB}}}{\partial q_j}=A_{{\mathrm{OT}}_j}{l}_{{\mathrm{T}}_j\mathrm{B}}{\dot{\theta }}_{{\mathrm{OR}}_j}+A_{{\mathrm{OT}}_j}{l}_{{\mathrm{T}}_j\mathrm{B}}{\dot{\theta }}_{{\mathrm{R}}_j{\mathrm{T}}_j}-A_{{\mathrm{OT}}_j}D{\dot{l}}_{{\mathrm{T}}_j\mathrm{B}}.$$

(25)

Similarly, the M end of the cut-off joint is associated with pin joint j. As a result, velocity ${\dot{r}}_{\mathrm{OB}}$ will remain unchanged despite any movement of the joint. The same as $\partial {\dot{r}}_{\mathrm{OB}}/\partial q_j$ holds true for $\partial {\dot{r}}_{\mathrm{OM}}/\partial q_j$, which is represented as

$$\begin{array}{l}\frac{\partial }{\partial q_j}{\dot{r}}_{\mathrm{OM}}=\frac{\partial }{\partial q_j}({\dot{r}}_{{\mathrm{OR}}_j}+{\dot{r}}_{{\mathrm{R}}_j{\mathrm{T}}_j}+{\dot{r}}_{{\mathrm{T}}_j\mathrm{M}})\\ =D{A}_{{\mathrm{OR}}_j}{\dot{\theta }}_{{\mathrm{OR}}_j}\frac{\partial A_{{\mathrm{R}}_j{\mathrm{T}}_j}}{\partial q_j}{l}_{{\mathrm{T}}_j\mathrm{M}}+A_{{\mathrm{OR}}_j}D\frac{\partial A_{{\mathrm{R}}_j{\mathrm{T}}_j}}{\partial q_j}{\dot{\theta }}_{{\mathrm{R}}_j{\mathrm{T}}_j}{l}_{{\mathrm{T}}_j\mathrm{M}}+A_{{\mathrm{OR}}_j}\frac{\partial A_{{\mathrm{R}}_j{\mathrm{T}}_j}}{\partial q_j}{\dot{l}}_{{\mathrm{T}}_j\mathrm{M}}\\ =-A_{{\mathrm{OT}}_j}{l}_{{\mathrm{T}}_j\mathrm{M}}{\dot{\theta }}_{{\mathrm{OR}}_j}-A_{{\mathrm{OT}}_j}{l}_{{\mathrm{T}}_j\mathrm{M}}{\dot{\theta }}_{{\mathrm{R}}_j{\mathrm{T}}_j}+A_{{\mathrm{OT}}_j}D{\dot{l}}_{{\mathrm{T}}_j\mathrm{M}}.\end{array}$$

(26)

By substituting Equation (26) into the partial derivative of Equation (6), the Jacobian submatrix reads as

$$\frac{\partial c}{\partial q_j}=\left[-A_{{\mathrm{OT}}_j}{l}_{{\mathrm{T}}_j\mathrm{M}}{\dot{\theta }}_{{\mathrm{OR}}_j}-A_{{\mathrm{OT}}_j}{l}_{{\mathrm{T}}_j\mathrm{M}}{\dot{\theta }}_{{\mathrm{R}}_j{\mathrm{T}}_j}+A_{{\mathrm{OT}}_j}D{\dot{l}}_{{\mathrm{T}}_j\mathrm{M}}\right].$$

(27)

The obtained Jacobian submatrices ${\dot{c}}_q$ are combined with $c_q$ to form the overall Jacobian matrix. The overall Jacobian matrix is then used in Equation (21) to correct generalized coordinates and velocities of the system with closed loops simultaneously.

5. Simultaneous Elimination of Position-Level, Velocity-Level, and Energy Constraint Violations

The violation correction process in physics and engineering refers to adjusting the system state $\left(q,\dot{q}\right)$ to ensure that constraints, such as the position and velocity, are met. The violation correction expects to minimize the impact of these adjustments on the dynamics of the system. The dynamics equation is essentially the first-order derivative of the energy conservation equation. Consequently, it is crucial to take the energy constraint equation into account. The system constraint equations $c_{\mathrm{E}}$ are now represented as

$$c_{\mathrm{E}}=\left[\begin{array}{c}c\\ \dot{c}\\ \Delta E\end{array}\right].$$

(28)

The Newton–Raphson formula for constraint $c_{\mathrm{E}}$ to correct the generalized coordinates and velocities is described as

$$c_{\mathrm{E}}+\frac{\partial c_{\mathrm{E}}}{\partial {\left[{\begin{array}{cc}{q}^{\mathrm{T}}& \dot{q}\end{array}}^{\mathrm{T}}\right]}^{\mathrm{T}}}\Delta \left[\begin{array}{c}q\\ \dot{q}\end{array}\right]=0.$$

(29)

5.1. Simultaneous Formulation for Correcting Position-Level, Velocity-Level, and Energy Constraints

The constraint equation of energy in a conservative system, disregarding any energy consumption or supplementation [29], can be expressed as follows:

$$\Delta E = E-E_0=0,$$

(30)

where $E$ is the system energy calculated at each correction point. $E_0$ is the initial energy of the system, which remains constant. The Jacobian matrices of the energy constraint equation for generalized coordinates and velocities $\left(q,\dot{q}\right)$ can be simplified to

$$\left\{\begin{array}{c}\frac{\partial \Delta E}{\partial q}=\frac{\partial \left(E-E_0\right)}{\partial q}=\frac{\partial E}{\partial q}\\ \frac{\partial \Delta E}{\partial \dot{q}}=\frac{\partial \left(E-E_0\right)}{\partial \dot{q}}=\frac{\partial E}{\partial \dot{q}}\end{array}\right..$$

(31)

The system energy is composed of the system kinetic energy and the system potential energy, denoted as

$$E = V+T,$$

(32)

where $V$ is the potential energy of the system and $T$ is the kinetic energy of the system. The system potential or kinetic energy is the sum of the potential or kinetic energy of each element, denoted as

$$V={\displaystyle \sum _{\mathrm{i}=1}^n{V}_{{\mathrm{H}}_i}}+{\displaystyle \sum _{\mathrm{i}=1}^n{V}_{{\mathrm{B}}_i}},$$

(33)

$$T={\displaystyle \sum _{\mathrm{i}=1}^n{T}_{{\mathrm{B}}_i}},$$

(34)

where $V_{{\mathrm{B}}_i}\left(V_{{\mathrm{H}}_i}\right)$ is the potential energy of the ith body(joint) element in the system, and n is the total number of the body or joint elements in the system. $T_{{\mathrm{B}}_i}$ is the kinetic energy of the ith body element in the system. It is important to note that the joint element serves solely as a constraint on the system and does not impact the potential energy of system.

The calculation of energy at each moment in the system is influenced by the generalized coordinates and velocities. To correct energy violations, it is necessary to adjust both simultaneously. The correction equation for the energy constraint, utilizing the Newton–Raphson iteration method, is referred to as

$$\Delta E+\frac{\partial E}{\partial q}\Delta q+\frac{\partial E}{\partial \dot{q}}\Delta \dot{q}=0.$$

(35)

By combining this with the Jacobian matrix $c_{\mathrm{s}}$ in Equation (21), the Jacobian matrix in Equation (29) can be written as

$$\frac{\partial c_{\mathrm{E}}}{\partial {\left[{\begin{array}{cc}{q}^{\mathrm{T}}& \dot{q}\end{array}}^{\mathrm{T}}\right]}^{\mathrm{T}}}=\left[\begin{array}{cc}{c}_q& O\\ {\dot{c}}_q& c_q\\ E_q& E_{\dot{q}}\end{array}\right].$$

(36)

The correction process will utilize the derived constraints at the position and velocity levels and their corresponding Jacobian matrices $c_q$, ${\dot{c}}_q$. The following section will derive the semi-analytical form of the Jacobian matrix $E_q$, with $E_{\dot{q}}$ required for the energy correction.

5.2. The Jacobian Matrix of the Constraint Equation

In the context of the simultaneous energy correction in Equation (36), the Jacobian matrix can be represented in a specific form:

$$\frac{\partial c_{\mathrm{E}}}{\partial {\left[\begin{array}{cccccc}{q}_1& \cdots & q_n& {\dot{q}}_1& \cdots & {\dot{q}}_n\end{array}\right]}^{\mathrm{T}}}=\left[\begin{array}{cccccc}{c}_{q_1}& \cdots & c_{q_n}& O& \cdots & O\\ {\dot{c}}_{q_1}& \cdots & {\dot{c}}_{q_n}& c_{q_1}& \cdots & c_{q_n}\\ E_{q_1}& \cdots & E_{q_n}& E_{{\dot{q}}_1}& \cdots & E_{{\dot{q}}_n}\end{array}\right].$$

(37)

The Jacobian matrices of the energy correction, as the energy equations illustrated in Equations (32)–(34), can be represented as the combination of the Jacobian matrix of potential energy and the Jacobian matrix of kinetic energy, read as

$$\left\{\begin{array}{c}\frac{\partial E}{\partial q_j}=\frac{\partial V}{\partial q_j}+\frac{\partial T}{\partial q_j}\\ \frac{\partial E}{\partial {\dot{q}}_j}=\frac{\partial V}{\partial {\dot{q}}_j}+\frac{\partial T}{\partial {\dot{q}}_j}\end{array}\right.,$$

(38)

where

$$\frac{\partial T}{\partial q_j}={\displaystyle \sum _{i=1}^n\frac{\partial T_{{\mathrm{B}}_i}}{\partial q_j}},$$

(39)

$$\frac{\partial V}{\partial q_j}={\displaystyle \sum _{i=1}^n\frac{\partial V_{{\mathrm{H}}_i}}{\partial q_j}}+{\displaystyle \sum _{i=1}^n\frac{\partial V_{{\mathrm{B}}_i}}{\partial q_j}}.$$

(40)

Determining the partial derivatives of the kinetic energy and potential energy for the $i\mathrm{th}\left(i=1,2\cdots n\right)$ body and joint with respect to the generalized coordinate and velocity $\left(q_j,{\dot{q}}_j\right)$ is necessary. These partial derivatives will be computed using a semi-analytic approach as outlined in the following subsection.

5.2.1. The Partial Derivative of the Potential Energy for Rigid Body i

The potential energy of the ith rigid element, assuming that the plane on which the ground allocate has a potential energy of zero, can be represented mathematically as

$$V_{{\mathrm{B}}_i}=-m_i{r}_{{\mathrm{OC}}_i}^{\mathrm{T}}g,$$

(41)

where $m_i$ is the mass of ith rigid body and $g$ is the acceleration due to gravity. This calculation is based on the consideration that gravity is the only applied force acting on the element.

The partial derivative of potential energy $V_{{\mathrm{B}}_i}$ with respect to the generalized coordinate and velocity of joint j yield

$$\frac{\partial V_{{\mathrm{B}}_i}}{\partial q_j}=m_i{\left(\frac{\partial r_{{\mathrm{OC}}_i}}{\partial q_j}\right)}^{\mathrm{T}}g,$$

(42)

$$\frac{\partial V_{{\mathrm{B}}_i}}{\partial {\dot{q}}_j}=m_i{\left(\frac{\partial r_{{\mathrm{OC}}_i}}{\partial {\dot{q}}_j}\right)}^{\mathrm{T}}g.$$

(43)

The mass and gravitational acceleration of the rigid element are known in the above equation. It is necessary to determine a semi-analytic expression for the partial derivatives of $r_{{\mathrm{OC}}_i}$ with respect to generalized coordinate $q_j$ and velocity ${\dot{q}}_j$. Similar to Equation (10), $r_{{\mathrm{OC}}_i}$ can be represented in terms of the kinematic quantities of joint j, written as

$$\begin{array}{l}{r}_{{\mathrm{OC}}_i}=r_{{\mathrm{OR}}_j}+r_{{\mathrm{R}}_j{\mathrm{T}}_j}+r_{{\mathrm{T}}_j{\mathrm{C}}_i}\\ =r_{{\mathrm{OR}}_j}+A_{{\mathrm{OR}}_j}{l}_{{\mathrm{R}}_j{\mathrm{T}}_j}+A_{{\mathrm{OR}}_j}{A}_{{\mathrm{R}}_j{\mathrm{T}}_j}{A}_{{\mathrm{OT}}_j}^{\mathrm{T}}{r}_{{\mathrm{T}}_j{\mathrm{C}}_i}.\end{array}$$

(44)

Kinematic quantities $r_{{\mathrm{OR}}_j}$ and $A_{{\mathrm{OR}}_j}$ at the root end of joint j are derived from the inboard body element and are unaffected by joint j. Displacement $A_{{\mathrm{OT}}_j}^{\mathrm{T}}{r}_{{\mathrm{T}}_j{\mathrm{C}}_i}$, described in the ${\mathrm{T}}_j$-fixed frame, varies with time but is independent of joint j. Therefore, the partial derivative of Equation (44) with respect to generalized coordinate $q_j$ can be represented as

$$\begin{array}{l}\frac{\partial r_{{\mathrm{OC}}_i}}{\partial q_j}=\frac{\partial \left(A_{{\mathrm{OR}}_j}{l}_{{\mathrm{R}}_j{\mathrm{T}}_j}+A_{{\mathrm{OR}}_j}{A}_{{\mathrm{R}}_j{\mathrm{T}}_j}{A}_{{\mathrm{OT}}_j}^{\mathrm{T}}{r}_{{\mathrm{T}}_j{\mathrm{C}}_i}\right)}{\partial q_j}\\ =A_{{\mathrm{OR}}_j}\frac{\partial l_{{\mathrm{R}}_j{\mathrm{T}}_j}}{\partial q_j}+A_{{\mathrm{OR}}_j}\frac{\partial A_{{\mathrm{R}}_j{\mathrm{T}}_j}}{\partial q_j}{A}_{{\mathrm{OT}}_j}^{\mathrm{T}}{r}_{{\mathrm{T}}_j{\mathrm{C}}_i}.\end{array}$$

(45)

The kinematic quantities of joint j, as described earlier, are

$$\frac{\partial l_{{\mathrm{R}}_j{\mathrm{T}}_j}}{\partial q_j}=0,\frac{\partial A_{{\mathrm{R}}_j{\mathrm{T}}_j}}{\partial q_j}=A_{{\mathrm{R}}_j{\mathrm{T}}_j}D.$$

(46)

Substituting Equation (46) into Equation (45) gives

$$\frac{\partial r_{{\mathrm{OC}}_i}}{\partial q_j}=D{r}_{{\mathrm{T}}_j{\mathrm{C}}_i}.$$

(47)

Since the quantities in Equation (44) are independent of the generalized velocity, the partial derivatives of the equation with respect to generalized velocity ${\dot{q}}_j$ will be

$$\frac{\partial r_{{\mathrm{OC}}_i}}{\partial {\dot{q}}_j}=0.$$

(48)

The partial derivative of potential energy $V_{{\mathrm{B}}_i}$ with respect to the generalized coordinate of joint j is given by the expression in

$$\frac{\partial V_{{\mathrm{B}}_i}}{\partial q_j}=m_i{\left(D{r}_{{\mathrm{T}}_j{\mathrm{C}}_i}\right)}^{\mathrm{T}}g=-m_i{r}_{{\mathrm{T}}_j{\mathrm{C}}_i}^{\mathrm{T}}Dg.$$

(49)

The partial derivative of potential energy $V_{{\mathrm{B}}_i}$ with respect to generalized velocity ${\dot{q}}_j$ is

$$\frac{\partial V_{{\mathrm{B}}_i}}{\partial {\dot{q}}_j}=0.$$

(50)

5.2.2. The Partial Derivative of the Kinetic Energy for Rigid Body i

The kinetic energy for rigid body i can be expressed as

$$T_{{\mathrm{B}}_i}=\frac{1}{2}{m}_i{\dot{r}}_{{\mathrm{OC}}_i}^2+\frac{1}{2}{\dot{\theta }}_{{\mathrm{OC}}_i}^2{J}_{{\mathrm{C}}_i},$$

(51)

where $J_{C_i}$ is the moment of inertia of the body about the center of mass and ${\dot{\theta }}_{{\mathrm{OC}}_i}^{}$ is the angular velocity of the body. The partial derivatives of the kinetic energy for the ith rigid body with respect to the generalized coordinate and velocity for joint j can be represented as

$$\begin{array}{l}\frac{\partial T_{{\mathrm{B}}_i}}{\partial q_j}=\frac{\partial }{\partial q_j}\left(\frac{1}{2}{m}_i{\dot{r}}_{{\mathrm{OC}}_i}^2+\frac{1}{2}{\dot{\theta }}_{{\mathrm{OC}}_i}^2{J}_{{\mathrm{C}}_i}\right)\\ =m_i{\dot{r}}_{{\mathrm{OC}}_i}^{\mathrm{T}}\frac{\partial {\dot{r}}_{{\mathrm{OC}}_i}}{\partial q_j}+{\left(\frac{\partial {\dot{\theta }}_{{\mathrm{OC}}_i}^{}}{\partial q_j}\right)}^{\mathrm{T}}{\dot{\theta }}_{{\mathrm{OC}}_i}^{}{J}_{{\mathrm{C}}_i},\end{array}$$

(52)

$$\begin{array}{l}\frac{\partial T_{{\mathrm{B}}_i}}{\partial {\dot{q}}_j}=\frac{\partial }{\partial {\dot{q}}_j}\left(\frac{1}{2}{m}_i{\dot{r}}_{{\mathrm{OC}}_i}^2+\frac{1}{2}{\dot{\theta }}_{{\mathrm{OC}}_i}^2{J}_{{\mathrm{C}}_i}\right)\\ =m_i{\dot{r}}_{{\mathrm{OC}}_i}^{\mathrm{T}}\frac{\partial {\dot{r}}_{{\mathrm{OC}}_i}}{\partial {\dot{q}}_j}+\left(\frac{\partial {\dot{\theta }}_{{\mathrm{OC}}_i}^{}}{\partial {\dot{q}}_j}\right){\dot{\theta }}_{{\mathrm{OC}}_i}^{}{J}_{{\mathrm{C}}_i}.\end{array}$$

(53)

Quantities ${\dot{r}}_{{\mathrm{OC}}_i}$ and ${\dot{\theta }}_{{\mathrm{OC}}_i}^{}$ in Equations (52) and (53) can be obtained using kinematic analysis. These quantities, along with the known quantities of mass and rotational inertia, are used to compute the overall influence of joint j on the kinetic energy of rigid body i. Similar to Equation (22), velocity ${\dot{r}}_{{\mathrm{OC}}_i}$ can be obtained by taking the derivative of Equation (44) with respect to time, read as

$$\begin{array}{l}{\dot{r}}_{{\mathrm{OC}}_i}={\dot{r}}_{{\mathrm{OR}}_j}+{\dot{r}}_{{\mathrm{R}}_j{\mathrm{T}}_j}+{\dot{r}}_{{\mathrm{T}}_j{\mathrm{C}}_i}\\ ={\dot{r}}_{{\mathrm{OR}}_j}+A_{{\mathrm{OR}}_j}^{}{l}_{{\mathrm{R}}_j{\mathrm{T}}_j}+A_{{\mathrm{OR}}_j}^{}{A}_{{\mathrm{R}}_j{\mathrm{T}}_j}{A}_{{\mathrm{OT}}_j}^{\mathrm{T}}{r}_{{\mathrm{T}}_j{\mathrm{C}}_i}\\ ={\dot{r}}_{{\mathrm{OR}}_j}+D{A}_{{\mathrm{OR}}_j}^{}{\dot{\theta }}_{{\mathrm{OR}}_j}{l}_{{\mathrm{R}}_j{\mathrm{T}}_j}+A_{{\mathrm{OR}}_j}^{}{\dot{l}}_{{\mathrm{R}}_j{\mathrm{T}}_j}+D{A}_{{\mathrm{OR}}_j}^{}{\dot{\theta }}_{{\mathrm{OR}}_j}{A}_{{\mathrm{R}}_j{\mathrm{T}}_j}{A}_{{\mathrm{OT}}_j}^{\mathrm{T}}{r}_{{\mathrm{T}}_j{\mathrm{C}}_i}\\ +A_{{\mathrm{OR}}_j}^{}D{A}_{{\mathrm{R}}_j{\mathrm{T}}_j}^{}{\dot{\theta }}_{{\mathrm{R}}_j{\mathrm{T}}_j}{A}_{{\mathrm{OT}}_j}^{\mathrm{T}}{r}_{{\mathrm{T}}_j{\mathrm{C}}_i}+A_{{\mathrm{OR}}_j}^{}{A}_{{\mathrm{R}}_j{\mathrm{T}}_j}\frac{\mathrm{d}}{\mathrm{d}t}\left(A_{{\mathrm{OT}}_j}^{\mathrm{T}}{r}_{{\mathrm{T}}_j{\mathrm{C}}_i}\right).\end{array}$$

(54)

Kinematic quantities ${\dot{r}}_{{\mathrm{OR}}_j}$, $A_{{\mathrm{OR}}_j}^{}$, ${\dot{\theta }}_{{\mathrm{OR}}_j}$, and $A_{{\mathrm{OT}}_j}^{\mathrm{T}}{r}_{{\mathrm{T}}_j{\mathrm{C}}_i}$ in Equation (54) remain unchanged despite any movement of joint j. The partial derivative of velocity ${\dot{r}}_{{\mathrm{OC}}_i}$ with respect to generalized coordinate $q_j$ can be represented as

$$\begin{array}{l}\frac{\partial {\dot{r}}_{{\mathrm{OC}}_i}}{\partial q_j}=D{A}_{{\mathrm{OR}}_j}^{}{\dot{\theta }}_{{\mathrm{OR}}_j}\frac{\partial l_{{\mathrm{R}}_j{\mathrm{T}}_j}}{\partial q_j}+A_{{\mathrm{OR}}_j}^{}\frac{\partial {\dot{l}}_{{\mathrm{R}}_j{\mathrm{T}}_j}}{\partial q_j}\\ +D{A}_{{\mathrm{OR}}_j}^{}{\dot{\theta }}_{{\mathrm{OR}}_j}\frac{\partial A_{{\mathrm{R}}_j{\mathrm{T}}_j}}{\partial q_j}{A}_{{\mathrm{OT}}_j}^{\mathrm{T}}{r}_{{\mathrm{T}}_j{\mathrm{C}}_i}+A_{{\mathrm{OR}}_j}^{}D\frac{\partial A_{{\mathrm{R}}_j{\mathrm{T}}_j}}{\partial q_j}{\dot{\theta }}_{{\mathrm{R}}_j{\mathrm{T}}_j}{A}_{{\mathrm{OT}}_j}^{\mathrm{T}}{r}_{{\mathrm{T}}_j{\mathrm{C}}_i}\\ +A_{{\mathrm{OR}}_j}^{}D{A}_{{\mathrm{R}}_j{\mathrm{T}}_j}\frac{\partial {\dot{\theta }}_{{\mathrm{R}}_j{\mathrm{T}}_j}}{\partial q_j}{A}_{{\mathrm{OT}}_j}^{\mathrm{T}}{r}_{{\mathrm{T}}_j{\mathrm{C}}_i}+A_{{\mathrm{OR}}_j}^{}\frac{\partial A_{{\mathrm{R}}_j{\mathrm{T}}_j}}{\partial q_j}\frac{\mathrm{d}}{\mathrm{d}t}\left(A_{{\mathrm{OT}}_j}^{\mathrm{T}}{r}_{{\mathrm{T}}_j{\mathrm{C}}_i}\right),\end{array}$$

(55)

where $\mathrm{d}\left(A_{{\mathrm{OT}}_j}^{\mathrm{T}}{r}_{{\mathrm{T}}_j{\mathrm{C}}_i}\right)/\mathrm{d}t$ described in the tip-fixed frame varies with time but does not change with $q_j$, described as

$$\begin{array}{l}\frac{\mathrm{d}}{\mathrm{d}t}\left(A_{{\mathrm{OT}}_j}^{\mathrm{T}}{r}_{{\mathrm{T}}_j{\mathrm{C}}_i}\right)={\left({\dot{A}}_{{\mathrm{OT}}_j}^{}\right)}^{\mathrm{T}}{r}_{{\mathrm{T}}_j{\mathrm{C}}_i}+A_{{\mathrm{OT}}_j}^{\mathrm{T}}{\dot{r}}_{{\mathrm{T}}_j{\mathrm{C}}_i}\\ ={\left(D{A}_{{\mathrm{OT}}_j}^{}{\dot{\theta }}_{{\mathrm{OT}}_j}\right)}^{\mathrm{T}}{r}_{{\mathrm{T}}_j{\mathrm{C}}_i}+A_{{\mathrm{OT}}_j}^{\mathrm{T}}{\dot{r}}_{{\mathrm{T}}_j{\mathrm{C}}_i}.\end{array}$$

(56)

The quantities of joint j, as previously described, are

$$\frac{\partial l_{{\mathrm{R}}_j{\mathrm{T}}_j}}{\partial q_j}=0,\frac{\partial A_{{\mathrm{R}}_j{\mathrm{T}}_j}}{\partial q_j}=A_{{\mathrm{R}}_j{\mathrm{T}}_j}D,\frac{\partial {\dot{\theta }}_{{\mathrm{R}}_j{\mathrm{T}}_j}}{\partial q_j}=0,\frac{\partial l_{{\mathrm{R}}_j{\mathrm{T}}_j}}{\partial {\dot{q}}_j}=0,\frac{\partial A_{{\mathrm{R}}_j{\mathrm{T}}_j}}{\partial {\dot{q}}_j}=0,\frac{\partial {\dot{\theta }}_{{\mathrm{R}}_j{\mathrm{T}}_j}}{\partial {\dot{q}}_j}=1.$$

(57)

The semi-analytic form of the partial derivative of $\partial {\dot{r}}_{{\mathrm{OC}}_i}/\partial q_j$ can be obtained by substituting Equation (57) into Equation (55). These partial derivatives can be represented as:

$$\frac{\partial {\dot{r}}_{{\mathrm{OC}}_i}}{\partial q_j}=-r_{{\mathrm{T}}_j{\mathrm{C}}_i}{\dot{\theta }}_{{\mathrm{OR}}_j}-r_{{\mathrm{T}}_j{\mathrm{C}}_i}{\dot{\theta }}_{{\mathrm{R}}_j{\mathrm{T}}_j}+A_{{\mathrm{OT}}_j}D\frac{\mathrm{d}}{\mathrm{d}t}\left(A_{{\mathrm{OT}}_j}^{\mathrm{T}}{r}_{{\mathrm{T}}_j{\mathrm{C}}_i}\right).$$

(58)

A well-known fact is that the partial derivatives $\partial r_{{\mathrm{OC}}_i}/\partial q_j$ and $\partial {\dot{r}}_{{\mathrm{OC}}_i}/\partial {\dot{q}}_j$ are equal. Referring to Equation (47), $\partial {\dot{r}}_{{\mathrm{OC}}_i}/\partial {\dot{q}}_j$ can be expressed as

$$\frac{\partial {\dot{r}}_{{\mathrm{OC}}_i}}{\partial {\dot{q}}_j}=D{r}_{{\mathrm{T}}_j{\mathrm{C}}_i}.$$

(59)

Quantity ${\dot{\theta }}_{{\mathrm{OC}}_i}^{}$ in Equations (52) and (53) can be expressed as

$${\dot{\theta }}_{{\mathrm{OC}}_i}={\dot{\theta }}_{{\mathrm{OR}}_j}+{\dot{\theta }}_{{\mathrm{R}}_j{\mathrm{T}}_j}+{\dot{\theta }}_{{\mathrm{T}}_j{\mathrm{C}}_i}.$$

(60)

The analysis conducted earlier indicates that the generalized coordinate and velocity exclusively impact ${\dot{\theta }}_{{\mathrm{R}}_j{\mathrm{T}}_j}$ in the above equation. Substituting Equation (57) into Equation (60), $\partial {\dot{\theta }}_{{\mathrm{OC}}_i}/\partial q_j,\partial {\dot{\theta }}_{{\mathrm{OC}}_i}/\partial {\dot{q}}_j$ yields

$$\frac{\partial {\dot{\theta }}_{{\mathrm{OC}}_i}}{\partial q_j}=\frac{\partial {\dot{\theta }}_{{\mathrm{R}}_j{\mathrm{T}}_j}}{\partial q_j}=0,\frac{\partial {\dot{\theta }}_{{\mathrm{OC}}_i}}{\partial {\dot{q}}_j}=\frac{\partial {\dot{\theta }}_{{\mathrm{R}}_j{\mathrm{T}}_j}}{\partial {\dot{q}}_j}=1.$$

(61)

Substituting Equations (58) and (61) into Equation (52) allows for the semi-analytic calculation of the partial derivative of the kinetic energy with respect to the generalized coordinate, represented as

$$\begin{array}{l}\frac{\partial T_{{\mathrm{B}}_i}}{\partial q_j}=m_i{\dot{r}}_{{\mathrm{OC}}_i}^{\mathrm{T}}\frac{\partial {\dot{r}}_{{\mathrm{OC}}_i}}{\partial q_j}+{\left(\frac{\partial {\dot{\theta }}_{{\mathrm{OC}}_i}^{}}{\partial q_j}\right)}^{\mathrm{T}}{\dot{\theta }}_{{\mathrm{OC}}_i}^{}{J}_{{\mathrm{C}}_i}\\ =m_i{\dot{r}}_{{\mathrm{OC}}_i}^{\mathrm{T}}\left(-r_{{\mathrm{T}}_j{\mathrm{C}}_i}{\dot{\theta }}_{{\mathrm{OR}}_j}-r_{{\mathrm{T}}_j{\mathrm{C}}_i}{\dot{\theta }}_{{\mathrm{R}}_j{\mathrm{T}}_j}+A_{{\mathrm{OT}}_j}D\frac{\mathrm{d}}{\mathrm{d}t}\left(A_{{\mathrm{OT}}_j}^{\mathrm{T}}{r}_{{\mathrm{T}}_j{\mathrm{C}}_i}\right)\right).\end{array}$$

(62)

The semi-analytic form of the partial derivative of kinetic energy with respect to the generalized velocity can be obtained by substituting Equations (59) and (61) into Equation (53). The expression is written as

$$\begin{array}{l}\frac{\partial T_{{\mathrm{B}}_i}}{\partial {\dot{q}}_j}=m_i{\dot{r}}_{{\mathrm{OC}}_i}^{\mathrm{T}}\frac{\partial {\dot{r}}_{{\mathrm{OC}}_i}}{\partial {\dot{q}}_j}+\left(\frac{\partial {\dot{\theta }}_{{\mathrm{OC}}_i}^{}}{\partial {\dot{q}}_j}\right){\dot{\theta }}_{{\mathrm{OC}}_i}^{}{J}_{{\mathrm{C}}_i}\\ =m_i{\dot{r}}_{{\mathrm{OC}}_i}^{\mathrm{T}}D{r}_{{\mathrm{T}}_j{\mathrm{C}}_i}+{\dot{\theta }}_{{\mathrm{OC}}_i}^{}{J}_{{\mathrm{C}}_i}.\end{array}$$

(63)

5.2.3. The Partial Derivative of the Potential Energy for Joint i

The potential energy of joint i can be described as

$$V_{{\mathrm{H}}_i}=\frac{1}{2}k{q}_i{}^2,$$

(64)

where $k$ is the stiffness coefficient of joint i. The potential energy $V_{{\mathrm{H}}_i}$ is affected by changes in joint j if $i=j$ holds. The partial derivative of the potential energy of joint j with respect to the generalized coordinate and velocity can be represented as

$$\frac{\partial V_{{\mathrm{H}}_j}}{\partial q_j}=k{q}_j^{},\frac{\partial V_{{\mathrm{H}}_j}}{\partial {\dot{q}}_j}=0.$$

(65)

6. Test Simulations

The precision of the method was evaluated using a conservative four-link system subjected to gravity as the only applied force (Figure 3). The system assumed identical link dimensions (1 m in length and 1 kg in mass), and the third joint was cut to create a span system with $q={\left[\begin{array}{ccc}{q}_1& q_2& q_3\end{array}\right]}^{\mathrm{T}}$. The dynamic calculations were performed using the RMSTMM, and the initial conditions were set as $q_1=\pi /3$, $q_2=-\pi /3$$,$ and $q_3=\pi /3$. A fixed-step fourth-order Runge–Kutta algorithm with a very small step size ($\Delta t=0.001$ s) was used for the numerical solution. The following four scenarios were conducted.

(a)

No constraint controls;

(b)

Direct elimination of position-level and velocity-level geometric constraint violations;

(c)

Simultaneous elimination of position-level and velocity-level geometric constraint violations;

(d)

Simultaneous elimination of position-level, velocity-level, and energy constraint violations.

Figure 3. Multibody model of a planar four bar mechanism.

Figure 3. Multibody model of a planar four bar mechanism.

Figure 4 illustrates the position constraint violations in the x-direction over time for different results. The violation in scenario (a) was divergent. Scenario (c) had a smaller constraint violation than scenario (b). Furthermore, scenario (d) had the smallest violations among all cases and was closest to zero.

Figure 5 shows the position constraint violations in the y-direction. The violations with scenario (b) were the largest among the correction scenarios. Scenario (c) managed to keep the constraint violations smaller, within the range of $\pm 0.3\times {10}^{-13}$. The violation of scenario (d) was the smallest among all cases, consistently staying within the range of $\pm 2\times {10}^{-16}$.

Figure 6 illustrates the velocity constraint violations in the x-direction for different scenarios. Scenario (b) only corrected the velocity constraint violations once, using Equation (4). As shown in the figure, the violations no longer diverged after correction. In contrast, scenario (c) included the velocity constraints in the constraint equations and corrected them, staying within a minor violation. Scenario (d) was notably closer to zero and demonstrated superior control.

The variation of the velocity constraint in the y-direction shown in Figure 7 became more pronounced, with a clear trend of divergence in the violations without correction. The constraint correction for the velocity level was applied once at each corrected moment in scenario (b), remaining within a range of $\pm 3\times {10}^{-13}$. Meanwhile, scenario (c) kept the violations within a range of $\pm 1\times {10}^{-13}$. Scenario (d) provided better accuracy of the violation at $\pm 0.3\times {10}^{-13}$.

It is important to note that non-conservation of the system energy can lead to calculation errors and inaccuracies. Therefore, correcting energy violations is crucial for achieving accurate and reliable results. Figure 8 illustrates the variations in energy violations under different scenarios. Scenario (a) showed an apparent change in energy. However, scenarios (b) and (c) provided a degree of correction to the energy violation, but the difference in energy variations between these scenarios was insignificant. Scenario (d) consistently penalized energy violations as expected, providing superior control and more robust energy stability.

Next, a complex closed-loop example, as depicted in Figure 9, was used to examine these scenarios. The system comprised rigid bodies with an identical mass of 1 kg and moment of inertia of $J_C=1/6 \mathrm{kg}\cdot {\mathrm{m}}^2$. The motion of the system was under the effect of gravity. The arrow direction in the figure indicates the transfer direction of the body element and its adjacent joint elements. The span tree system was obtained by cutting joints 13, 14, and 15. The simulation was conducted based on the RMSTMM using the fourth-order Runge–Kutta algorithm with a small time step of 0.001 s. Four different test scenarios were carried out:

(a)

No constraint controls;

(b)

Direct elimination of position-level and velocity-level geometric constraint violations;

(c)

Simultaneous elimination of position-level and velocity-level geometric constraint violations;

(d)

Simultaneous elimination of position-level, velocity-level, and energy constraint violations.

The simulation results were analyzed and compared to evaluate the performance of the different scenarios.

Figure 9. A multibody system with closed loops.

Figure 9. A multibody system with closed loops.

The behavior of the system’s violations over time was still observed using the different correction scenarios. The position and velocity constraints of joints 13, 14, and 15 were well corrected compared to the uncorrected scenario. However, the overall trends remained consistent. As an illustration, the trend of hinge element 15 in Figure 10 is presented, providing a clear view of the corrections. Figure 10 includes the following:

(1)

Position constraint violations in the x-direction;

(2)

Position constraint violations in the y-direction;

(3)

Velocity constraint violations in the x-directional;

(4)

Velocity constraint violations in the y-directional.

The deviation on the velocity level was more significant than the position level. By comparing scenarios (b), (c), and (d), scenarios (c) and (d) had the best correction performance and could effectively control the violations, while scenario (b) had a weaker correction performance. In long-term simulations, scenario (b) struggled to effectively control the violations. In contrast, scenario (c) was seen to control the violations effectively and consistently compared to scenario (b). Furthermore, scenario (d) exhibited the smallest violations and was closest to the 0-axis, making it the most effective correction scenario.

Figure 10. The violation of constraints in joint 14. (1) Position constraint violations in the x-direction; (2) Position constraint violations in the y-direction; (3) Velocity constraint violations in the x-directional; (4) Velocity constraint violations in the y-directional.

Figure 10. The violation of constraints in joint 14. (1) Position constraint violations in the x-direction; (2) Position constraint violations in the y-direction; (3) Velocity constraint violations in the x-directional; (4) Velocity constraint violations in the y-directional.

Energy changes can also have an immeasurable impact on simulations. Figure 11 illustrates the energy changes under four different scenarios: (a), (b), (c), and (d). Without correction, as time progressed, the energy curve of scenario (a) exhibited a clear diverging trend. Scenario (b), which corrected the velocity violation without iteration, also caused the energy to diverge. Scenario (c) did not show a significant divergence in energy, but the violation was still present. In contrast, scenario (d) effectively corrected the energy as expected. This correction reduced the energy violation and minimized its effect on numerical integration during long-term simulations.

7. Conclusions

This article discussed direct correction methods in violation problems of the dynamic methods, which avoid global dynamics equations with a system inertia matrix. The general direct correction only corrects velocity constraints without consecutive iteration, but this article suggested a method that simultaneously corrects constraints at both the position and velocity level. Energy violations can potentially lead to system failure in long-time simulations. Hence, the article discussed a new approach for correcting dynamics systems violation problems that consider constraints at both the position and velocity level, along with changes in the system’s energy. This approach improved the accuracy and robustness of simulation results by taking into account all constraints, resulting in stable and predictable system behavior in actual operation.

This article attempted to correct the energy while also correcting position and velocity constraints, but the effect of energy on violation issues remains worth investigating. The potential future research objectives are as follows: (1) One of the apparent areas for improvement of energy constraint control methods is the accuracy of determining E for non-conservative systems [38]. It is worth delving into how to correct energy for non-conservative systems and the derivation of its correction Jacobian matrix in a semi-analytical form. (2) The increasing use of lightweight and flexible materials in mechanical systems offers many benefits but presents challenges when modeling and analyzing the systems. These materials exhibit complex behavior, and modeling them using kinetic methods can lead to intricate DAEs. Therefore, it is an important area of research in the future to investigate the effect of energy corrections on flexible bodies to ensure accurate and reliable analysis.