Multibody model of the human-inspired robot CHARMIE

Abstract

The rapid ageing of the worldwide population raises pressing concerns related to ensuring proper healthcare and quality of life for older adults. A human-like mobile domestic robot, named CHARMIE, is being produced to aid in these situations by performing household chores, thus increasing the autonomy of persons with mobility limitations. The present work provides a valuable contribution to the development of CHARMIE by building a simulation environment that computes the system’s main dynamics. The obtained environment is used to evaluate the quality of the robot’s control system, to perform its structural optimization and to allow a proper selection of actuators. The system is tackled as a kinematic tree that starts on the robot’s base and then splits into three branches at the torso: the left arm, the right arm, and the head. The multibody model solves the forward kinematics and inverse dynamics of the main mechanisms by employing two recursive algorithms centred around the Newton–Euler formulation. A novel, modular, and efficient seven-step methodology was created to implement these two algorithms and program a simulator from start to finish. These seven steps include studying the system’s configuration, converting its properties into software inputs, and computing the phenomena that cannot be automatically addressed by the two recursive formulations. The presented methodology was fully validated by comparing its results to those obtained from a commercial software; the two models produced identical results.

1 Introduction

One of the most pressing concerns of modern societies is the rapid aging of the worldwide population [1]. With the recent developments in robotic technologies, robots can play a growing role in ensuring the quality of life of older adults by increasing their autonomy, for example, by performing daily chores [2]. CHARMIE, a novel human-inspired mobile manipulator (see Fig. 1) [3], was developed to aid in tackling this issue.

Fig. 1

Physical prototype (a) and CAD model (b) of the CHARMIE robot

Multibody dynamic simulations are crucial for the research of robotic systems, namely by allowing the study of the robot’s motion and trajectories, the efficiency comparison of different control solutions, and the design optimization of the mechanism’s bars, joints, and actuators [4]. This work focuses on the development of CHARMIE’s multibody computational model.

The methods for analyzing multibody systems can be grouped into two categories in which: i) a commercial (or open-source) software is used to create the computational model, or ii) the equations of motion are derived and then implemented into an integrated development environment. The survey conducted in [5] shows that humanoid robot researchers mostly use method (i), justifying this decision with the usefulness of the various tools provided by the software, the quality of the user interfaces, and the ability to tackle complex systems without requiring in-depth studies. Method (ii) demands a more laborious setup stage, but also provides powerful advantages such as allowing an optimization of the code to more efficiently tackle specific issues, giving the programmer full control over the simulation environment, and producing models with higher compatibility with external computational tools. The choice of the best method ultimately depends on the nature of the project and the intended applications of the virtual model. As an example, graphical interfaces and computer vision problems are easier to deal with using method (i), while the greater customizability granted by method (ii) allows for quicker parametric optimization studies. CHARMIE’s multibody model has three main uses, namely determining actuator loads, performing parametric optimization of static balancing mechanisms, and training neural network control solutions for trajectory control. The two methods are equally valid for the first study, but the second and third studies benefit greatly from the use of adequately implemented own code, therefore, this was the chosen methodology for the CHARMIE project.

The forward kinematics of this system was solved by using the recursive algorithm presented in [6]. In this method, bodies are first modeled in their local coordinates, then rotated using rotation matrices defined by Euler angles, and finally translated into their final positions. The inverse dynamic analysis is then carried out by using the recursive Newton–Euler algorithm of [7] expressed in the mathematical notation of [4]. The present work further adapts the formulation of [4] to allow it to: tackle tree structure kinematic chains; integrate formulations that solve closed loop systems; and permit any choice of axis-orientations, avoiding the limits imposed by the Denavit–Hartenberg parameters. The aforementioned recursive algorithms are integrated into a seven-step methodology (Fig. 2), which was used to assemble the multibody model of CHARMIE.

Fig. 2

Flowchart describing the proposed seven-step method for the multibody computational analysis

The main contribution of this work is twofold. First, it addresses a pressing concern in an ongoing project, namely the need for an in-house optimized code to achieve the multibody model of CHARMIE, which is required for performing tests of various control solutions and to study the optimization of the robot mechanisms. Second, the work describes and validates a systematic seven-step methodology (see Fig. 2) intended to simulate complex articulated robotic systems. This methodology groups and interconnects mathematical formulations known from the literature to create a complete multibody simulation from start to finish. The feasibility of the proposed approach was confirmed by employing it to a specific case study. By comparing the used methodology with the methods described in the literature, three main advantages were identified: i) It groups a full set of formulations that can be applied to build a complete dynamics simulator; ii) The obtained simulator has a high computational efficiency [8]; and iii) The resulting program is extremely modular and adaptable.

The remainder of this work begins with a literature review in Sect. 2, where the choice of used mathematical formulations is justified, and the main available alternatives to tackle the system’s sub-problems are described. The seven steps of the new proposed methodology are grouped into three main parts, which correspond to the following sections of the paper: Sect. 3, constructing the multibody model and preparing it by converting its properties into the relevant inputs for the recursive algorithms; Sect. 4, analysing the forward kinematics, including the geometric study of indirectly actuated joints, implementing the recursive algorithm for the kinematics analysis, and solving the kinematics of additional bodies; and Sect. 5, analysing the inverse dynamics, including employing the Newton–Euler recursive algorithm and solving the dynamics of closed and over-constrained loops. Section 4 does not require any input from Sect. 5, allowing the program to be utilized for purely kinematic analyzes (for example, for trajectory optimization). In Sect. 6 the developed methodology is validated by comparing the obtained outcomes with those produced from a commercial software. Thus, Sect. 7 concludes the work by presenting suggestions for future work and concluding remarks regarding the methodology’s performance and applications.

2 Literature review

The goal of this literature review is to present alternative notations, which could have been used to model CHARMIE, to justify the choice of notation used, and to compare it to the listed alternatives. First, forward kinematics is presented, then methodologies for the inverse dynamics, and finally the multibody problem as a whole is analyzed.

The kinematics of a body is modeled by defining its position and orientation, as well as how these properties vary over time. The most employed method for the kinematic study of multibody robotic systems is the Denavit–Hartenberg parameters, in which four scalar variables define a [4 × 4] homogeneous transformation matrix that describes the rotation and translation between two consecutive joints in a serial chain of bodies [9]. Despite its popularity, this method possesses its own set of drawbacks, such as limiting the choice of orientation for the axes, which can make software outputs harder to interpret, and using homogeneous transformation matrices that reduce the computational efficiency by increasing the number of multiplications by ones and zeros [10]. This second limitation can be addressed by dividing the translation and rotation operations into two sequential calculations. Positions and displacements in Euclidean space can be simply defined using three-dimensional Cartesian coordinates, however, orientations and rotations provide a more complex problem. Rotation matrices ($[3 \times 3]$ matrices for three-dimensional space) are commonly utilized to define the rotation between two references because they are simple to understand, implement, and manipulate. One of the several methods used alongside rotation matrices is the Euler angles, a set of three predefined rotations that can represent any orientation in space [11]. Some researchers avoid using Euler angles due to their two well-known limitations: gimbal lock and the Euler angle singularity [12]. An alternative to avoid these limitations is to use quaternions, where rotations are defined by a vector with four scalars [13]. Quaternions are often favored for their high computational efficiency [14]. Another common approach to multibody systems is screw theory, where two three-dimensional vectors characterize the Plücker coordinates of a line in space, and the magnitude of a screw and its pitch, which can be harnessed to describe the position and orientation of a rigid body [15]. Screw theory may be used alongside Lie Group algebra to obtain general and highly efficient recursive algorithms for spatial algebra [16]. A common application of screw theory within the field of multibody dynamics is the use spatial vector quantities to develop kinematics and dynamic algorithms based on spatial vector algebra [8, 17]. Of the possibilities listed above (only some of several methodologies), the forward kinematics analysis of CHARMIE uses Euler angles [6]. Since a constrained system is studied, the gimbal lock and singularity problems of Euler angles are avoided by choosing the correct axis orientations [12]. This notation is also advantageous since it directly outputs the required inputs for the selected dynamic analysis method [18].

The goal of the inverse dynamic analysis of multibody systems is to determine the loads applied by each pair of connected bodies based on the known kinematics and physical properties of the system. Several main formulations are applicable for obtaining the equations of motion of dynamic systems: the Newton-Euler equations [19], the Lagrange equations [20], the Hamilton equations [21], and the equations from Kane’s method [22]. In robotics, recursive Newton-Euler algorithms [7] are mainly used due to their simplicity and high computational efficiency (the number of calculations increases linearly with the number of bodies in the system) [17]. Recursive Newton–Euler algorithms are divided into two stages: first, iterations progress from the ground reference to the end-effectors to obtain the forward kinematics of each body; then, iterations progress from the end-effectors to the ground reference to calculate the forces and torques applied between each pair of connected bodies. The work by [23] derives highly efficient recursive Newton–Euler algorithms structured around screw theory and Lie algebra. In [17], the recursive Newton–Euler algorithm is adapted to work with spatial vector algebra, but the author notes that this causes a reduction in computational efficiency (in comparison to the standard notation) since it requires calculating the linear velocity of all bodies. One of the Newton–Euler notations with the highest computational efficiency is described in [24]. The efficiency of this notation was matched by [25], which employs Kane’s equations to solve the dynamics of manipulators with only revolute or prismatic joints, modeled using the Denhavit–Hartenberg parameters. Parallel computation based notations are more commonly used for forward dynamics problems [26–28], but they can also be applied to further improve the computational effiency of inverse dynamics problems [29]. CHARMIE was modeled using the Newton-Euler formulation of [7] expressed in the mathematical notation of [4]. This base formulation is quite simple, and it is expressed as an algorithm which inputs joint properties and the rotation matrices between consecutive bodies to address serial kinematic systems modeled using Denhavit–Hartenberg parameters. This algorithm was expanded in Sect. 5 of this work to better perform the inverse dynamic analysis of CHARMIE.

The two aforementioned analyzes, direct kinematics and inverse dynamics, must both be performed to achieve the desired complete multibody model. These two studies are intertwined, so picking the most computationally efficient model for each problem independently may result in a non-optimal solution (due to the required computational effort of converting between notations). Therefore, several works in the literature are prepared to tackle these problems as a whole, for example, [17] expresses the equations using spatial vector algebra, while [30] uses spatial kernel operators (SKO) and spatial propagation operators (SPO) for the multibody analysis of complex systems.

The analysis of CHARMIE must solve two specific problems, namely tree structures [31] and closed loops [32]. Diverse methods have been presented to systematically solve tree-like kinematic structures, such as [16, 17, 30, 31]; also, formulations for tackling serial kinematic chains can be easily extended to address tree kinematic structure [8]. Depending on the complexity of the system, the solution of closed loops can become a more intricate problem, for example, the SKO models in [30] have issues in dealing with closed loops. Nonetheless, authors have described and integrated into their models systematic methods of dealing with closed loops; the work by [17] describes a set of strategies for solving closed systems, the work by [32] completes the model it presented in [31] to allow it to tackle closed chains, in [33] the divide-and-conquer (DCA) method is extended for the inverse dynamic analysis of open and closed loops, and [34] develops an efficient method to study the inverse dynamics of manipulators based on the Newton–Euler equations and the virtual work principle.

In the present work, the proposed systematic seven-step approach requires a more in-depth analysis of the target of the study in comparison to more generalized formulations, which are prepared for any geometry. The advantages of developing a more specific solution for the required problem are that a high-efficiency inverse dynamics algorithm could be used [17], and that the closed loops were tackled with more specific notations leading to higher computational efficiency. The proposed approach integrates the solution of several key issues in obtaining a multibody simulator, namely tackling tree structure kinematic chains and closed loops, implementing the equations of motion, and generating a graphical output. The resulting program is highly modular, which provides three main advantages: other formulations from the literature review can be tested at a later stage to further increase computational efficiency; functions can be disabled for more focused studies (such as performing a trajectory optimization based on kinematics only); and additional mathematical notations that interact with the model can be added, for example, in an ongoing study, CHARMIE’s locomotion was modeled using a direct dynamic approach, which was fully integrated within this model.

3 Construction and preparation of the multibody model

The initial step for creating the multibody model of CHARMIE is to define each of the robot’s bodies and joints. First, bodies are labeled according to their position in the kinematic chain. Then, using information obtained from the robot’s CAD model, data defining the geometry, mass, and inertia of the bodies, as well as the orientation, type, and position of the joints are computed and serve as input to the recursive algorithms.

3.1 Description of the multibody model

CHARMIE’s structure is divided into five main sections. From the base to the end-effector, these are: i) the omnidirectional base, which includes the suspension and locomotion systems; ii) the 2 DOF (degrees of freedom) articulated lower and upper body of the robot, responsible for increasing the robot’s workspace; iii) the left arm and iv) the right arm, each with 6 DOF for their motion, and an additional one for opening and closing a claw-like end-effector; and v) the robot’s head with a 2 DOF neck. In the current stage of the virtual model, the kinematics and dynamics of the locomotion and suspension systems are simplified, represented as three DOFs that allow the robot to slide and rotate along the ground plane. This results in a total of 21 DOFs, which are actuated by 16 revolute motors, two linear actuators, and three additional fictitious actuators (two linear and one revolute) to control the locomotion.

The complete assembly of the five sections results in a total of 40 rigid bodies, interconnected by 34 revolute joints, ten prismatic joints, and three rigid joints (see Fig. 3). Bodies with varying lengths, namely linear actuators or tension springs, are represented as two bodies connected by a prismatic joint.

Fig. 3

Kinematic model of the CHARMIE mobile manipulator robot

The proposed methodology requires identifying the main kinematic chains of the studied system. These chains progress from base to end-effector; links $\text{i}-1$ are considered as preceding link i, while links $\text{i} + 1$ are succeeding link i. In CHARMIE’s case, the bodies from the floor plane to the robot’s upper body (links 1–5) constitute a single serial kinematic chain. This main chain is then split into three that represent the left arm (links $6_{\text{a}}$–$13_{\text{a}}$), the right arm (links $6_{\text{b}}$–$13_{\text{b}}$), and the head (links $6_{\text{c}}$–$8_{\text{c}}$), respectively. The indirect actuation and static balancing systems placed between body pairs 3–4 and 4–5 create closed and over-constrained loops that must be tackled separately in the dynamic analysis. Figure 3 distinguishes the bodies that belong to the main kinematic chains from those that are auxiliary by labeling the main ones with bold letters.

3.2 Conversion of the body properties into software inputs

The recursive algorithms for the kinematics and dynamic analyzes require two main types of inputs. The first type is the position, velocity, and acceleration of each actuator. These spatial properties are defined by the robot’s trajectory planning and control (which can either be predetermined or updated during the execution of the simulation). The second type of input is the physical and geometrical properties of the robot, which include each body’s dimension, shape, mass, and inertia, as well as the type, position, and orientation of all joints. All these characteristics must be completely defined for bodies belonging to the main kinematic chains.

The first defined property was the shape of each body. This process is started by associating a local coordinate axis with origin $\text{O}_{\text{i}}$ to body i. Then, a point cloud is used to describe the required shapes. At this stage, a reduced number of points were defined manually to convey the key information of the geometries. Bodies are modeled in the point cloud, choosing the most convenient axis direction to simplify the robot’s assembly and description (for example, for the robot’s arms, the $z$ axis represents the length of all parts). In the equations throughout this work, these points can be expressed regarding different coordinate axes using the following notation:

• $\textbf{P}'_{\text{i}}$: the coordinates of point P of body i, expressed in the local axis of i.

• $\textbf{P}''_{\text{i}}$: the coordinates of point P of body i, expressed in an auxiliary axis that represents body i rotated around $\text{O}'_{\text{i}}$ to its current global orientation considering the configuration of the robot.

• $\textbf{P}_{\text{i}}$: the coordinates of point P of body i, expressed in the global coordinate axis.

• ${\textbf{P}}^{\prime \,\text{j}}_{\text{i}}$: the coordinates of point P of body i, expressed in the local axis of body j.

Besides the origin, a set of important points must be identified and kept coherent for all bodies so that the equations are simpler to implement. Of these points, $\text{A}_{\text{i}}$ always indicates the connection between body i and its predecessor in the kinematic chain. If a revolute joint precedes body i, point $\text{A}_{\text{i}}$ is fixed and overlaps with $\text{O}_{\text{i}}$. If a prismatic joint precedes i, the position of $\text{A}_{\text{i}}$ in local coordinates is calculated using:

$$ \textbf{A}'_{\text{i}} = \textbf{O}'_{\text{i}} + d_{i} \textbf{z}_{ \text{i}} $$

(1)

where $d_{i}$ is the displacement between the origin of the body and the contact point between body i and its predecessor in the kinematic chain, and $\textbf{z}_{\text{i}}$ is the unit vector defining the orientation of the joint preceding i expressed in local coordinates (better defined in Equation (28)).

The algorithms also require defining points $\text{B}_{\text{i}}$, which describe the position of the joints between body i and its successors in the kinematic chain, and points $\text{C}_{\text{i}}$, which represent the coordinates of the centre of mass of body i (determined using the CAD model). Points $\text{O}_{\text{i}}$, $\text{A}_{\text{i}}$, $\text{B}_{\text{i}}$, and $\text{C}_{\text{i}}$ are sufficient to fully define geometries for executing the recursive algorithms. Any number of additional points can be used to better describe each body’s shape. Table 1 and Fig. 4 represent the points used for modelling CHARMIE’s body 5.

Fig. 4

(a) CAD model of CHARMIE’s body 5, and (b) CHARMIE’s body 5 in the simulation environment depicted in local coordinates by the key points described in Table 1

Table 1 Point coordinates defining the geometry of CHARMIE’s body 5, whose visual representation is shown in Fig. 4

With the shapes fully characterized, the focus then becomes the joints between them. Each joint was described using three variables. $JT_{\text{i}}$ defines the joint preceding body i as P—Prismatic, R—Revolute, or F—Fixed. The axis this joint revolves around, or slides along, is then identified with variable $JO_{\text{i}}$. Finally, using information from the body’s geometry, vector $\textbf{r}^{i}_{\text{Ai,Bi}}$ expresses the displacement between the preceding and succeeding joints of body i in its local coordinates. The values of these three variables for all main joints of CHARMIE are shown in Table 2.

Table 2 Description of CHARMIE’s joints by type ($JT_{\text{i}}$), orientation ($JO_{\text{i}}$), and position ($\textbf{r}^{i}_{\text{Ai,Bi}}$)

Three fixed joints were introduced into body 5 of CHARMIE to perform constant rotations, allowing more natural choice of axis orientations for the sub-kinematic chains a, b, and c. The changes in orientation were defined as intrinsic ZXZ-Euler rotations (see Equation (17)), as shown in Equations (2)–(4).

$$\begin{aligned} \textbf{R}^{\text{5}}_{\text{6a}} &= \text{ZXZEuler}(\uppi /2, -\uppi /2, \uppi ), \end{aligned}$$

(2)

$$\begin{aligned} \textbf{R}^{\text{5}}_{\text{6b}} &= \text{ZXZEuler}(\uppi /2, \uppi /2, 0), \end{aligned}$$

(3)

$$\begin{aligned} \textbf{R}^{\text{5}}_{\text{6c}} &= \text{ZXZEuler}(0, 0, 0). \end{aligned}$$

(4)

The aforementioned variables are sufficient to study the kinematics of the robot. For the dynamic analysis, it is also necessary to define the masses $m_{i}$ (Table 3) and inertia matrices $\bar{\textbf{I}}_{i}^{i}$ (Table 4). To simplify the system, bodies that do not belong to the main kinematic chains were considered massless. The system’s total mass was maintained by adding the removed masses into adjacent bodies of the main kinematic chains. The positions of the centers of mass were defined in the local coordinates of each body i using vector $\textbf{r}^{i}_{\text{Ai,Ci}}$, which expresses the displacement between the body’s joint with its preceding link and its center of mass.

Table 3 Mass ($m_{\text{i}}$) and center of mass position ($\textbf{r}^{i}_{ \text{Ai,Ci}}$) of CHARMIE’s bodies

Table 4 Physical properties defining the inertia matrices ($\bar{\textbf{I}}_{i}^{i}$) of CHARMIE’s bodies

The definition of all inputs listed above concludes the preparatory stages for the robot’s multibody simulation.

4 Forward kinematics analysis

The forward kinematics analysis uses the actuator positions, velocities, and accelerations defined by the trajectory, as well as the inputs that describe the shape of the bodies and joints, to determine the motion of the robot over time. Since the used recursive algorithm [6] requires the configuration of the joints, not the actuators, this analysis is initialized by studying the behavior of the indirectly actuated joints. Subsequently, the recursive algorithm can be utilized to obtain the kinematics of the main bodies. Finally, the motion of the additional bodies, such as springs and linear actuators, is determined to fully characterize the robot’s kinematics.

4.1 Geometric analysis of the indirectly actuated joints

In this section, a geometric approach is used to study the two indirectly actuated joints of CHARMIE, which are placed preceding bodies 4 and 5. At this stage of the methodology, each body is defined in local coordinates, but the full model has yet to be assembled. Since distinct references are being considered, the distance between points from different bodies cannot be used.

The joint preceding body 4 is a prismatic joint controlled by a linear actuator. The goal is to determine the joint displacement $d_{4}$ as a function of the actuator length $act_{4}$ (see Fig. 5a). Since the two bodies only move linearly along the $z$-axis, coordinates in the two local $y$-axes can be compared directly. The joint displacement is expressed as:

Fig. 5

Geometric parameters required for the analysis of CHARMIE’s indirectly actuated joints, namely (a) the joint preceding body 4 and (b) the joint preceding body 5

$$ d_{4} = A'_{4}z - O'_{4}z. $$

(5)

$A'_{4}z$ cannot be determined in its local coordinates without the joint displacement, but by expressing both $A'_{4}z$ and $O'_{4}z$ in relation to body 3, it is possible to establish $d_{4}$ as a function of $act_{4}$. $A'_{4}z$ in body 3’s reference corresponds directly to point $B'_{3}z$. The coordinates of $O'_{4}z$ can then be determined by starting in point $\text{D}_{\text{3}}$ and adding the vertical displacement caused by the linear actuator (obtained using the Pythagorean theorem), then subtracting the vertical displacement between point $\text{E}_{\text{4}}$ and point $\text{O}_{\text{4}}$, defined by the constant geometry of body 4. This results in Equation (6).

$$ d_{4} = B'_{3}z - \left (D'_{3}z+\sqrt{{act_{4}}^{2}-(E'_{3}y - D'_{3}y)^{2}}-(E'_{4}z-O'_{4}z) \right ). $$

(6)

This expression solves the required kinematic relation for the joint preceding body 4.

The joint preceding body 5 is a revolute joint controlled by a linear actuator. The goal is to determine the joint angular rotation $\vartheta _{5}$ as a function of the actuator length $act_{5}$ (see Fig. 5b). The value of $\vartheta _{5}$ can be obtained with the auxiliary angles $\alpha _{4}$ and $\beta _{5}$ using the expression:

$$ \vartheta _{5} = \alpha _{4}+\beta _{5}-\frac{\uppi}{2}. $$

(7)

Angle $\beta _{5}$ is constant, defined by the geometry of body 5 as:

$$ \beta _{5} = \text{arctan}\left ( \frac{G'_{5}y-A'_{5}y}{G'_{5}z-A'_{5}z} \right ). $$

(8)

Angle $\alpha _{4}$ varies over time with the configuration of the linear actuator. By expressing the coordinates of its endpoint $\text{G}_{\text{5}}$ in the reference of body 4 ($\text{G}_{\text{4}}$), this angle can be formulated as:

$$ \alpha _{5} = \text{arctan}\left ( \frac{G'_{4}z-B'_{4}z}{G'_{4}y-B'_{4}y} \right ). $$

(9)

The local coordinates of $\text{B}_{\text{4}}$ are defined by the geometry of body 4. The position of $\text{G}_{\text{4}}$ is calculated as the intersection between two circumferences, one with center $\text{F}_{\text{4}}$ and radius $act_{5}$, and the other with center $\text{B}_{\text{4}}$ and radius equal to the distance between $\text{B}_{\text{4}}$ and $\text{G}_{\text{4}}$. To determine the circumferences’ intersections, three auxiliary parameters, $e_{4}$, $l_{4}$, and $h_{4}$, are calculated:

$$\begin{aligned} e_{4} &= \sqrt{(F'_{4}y-B'_{4}y)^{2}+(F'_{4}z-B'_{4}z)^{2}}, \end{aligned}$$

(10)

$$\begin{aligned} l_{4} &= \frac{{act_{5}}^{2}-\left [ (G'_{5}y-A'_{5}y)^{2} + (G'_{5}z-A'_{5}z)^{2} \right ]+{e_{4}}^{2}}{2e_{4}} , \end{aligned}$$

(11)

$$\begin{aligned} h_{4} &= \sqrt{{act_{5}}^{2}-{l_{4}}^{2}}. \end{aligned}$$

(12)

It then becomes possible to determine the coordinates of $\text{G}_{\text{4}}$ using Equations (13)–(14).

$$\begin{aligned} G'_{4}y &= \frac{l_{4}}{e_{4}}(B'_{4}y-F'_{4}y)+\frac{h_{4}}{e_{4}} (B'_{4}z-F'_{4}z) + F'_{4}y, \end{aligned}$$

(13)

$$\begin{aligned} G'_{4}z &= \frac{l_{4}}{e_{4}}(B'_{4}z-F'_{4}z)+\frac{h_{4}}{e_{4}} (B'_{4}y-F'_{4}y) + F'_{4}z. \end{aligned}$$

(14)

By combining Equations (7)–(14), the relation between the configuration of the joint preceding body 5 and its linear actuator is established.

4.2 Recursive algorithm for the forward kinematics analysis of the main bodies

The forward recursive algorithm determines the position and orientation of all bodies that are a part of the main kinematic chains of CHARMIE by using the joint position, orientation, type and configuration. The algorithm models one body at a time, starting on the robot’s base (body 1), and progressing along the kinematic chain until the end-effectors (bodies 13a, 13b, and 8c) are reached. Each body is modeled following three sequential steps.

The first step is creating a model of body i in its local coordinates. For CHARMIE, these models were obtained using the coordinates of key points, as explained in Sect. 3.2.

The second step is rotating body i around its origin (Point $\text{O}_{\text{i}}$) so it assumes its current orientation concerning the global reference. This is achieved using the expression:

$$ \textbf{P}''_{\text{i}} = \textbf{R}^{\text{0}}_{\text{i}} \textbf{P}'_{ \text{i}}, $$

(15)

where each point $\text{P}_{\text{i}}$ is rotated using the $\textbf{R}^{\text{0}}_{\text{i}}$ rotation matrix that defines the orientation of body i.

The $\textbf{R}^{\text{0}}_{\text{i}}$ rotation matrix is calculated from the combination of two rotations: matrix $\textbf{R}^{\text{0}}_{{\text{i}-1}}$ that describes the orientation of the body preceding i (obtained in the previous iteration of the recursive algorithm); and matrix $\textbf{R}^{{\text{i}-1}}_{\text{i}}$ that defines the rotation between the preceding body and the current body. This process is formulated as:

$$ \textbf{R}^{\text{0}}_{\text{i}} = \textbf{R}^{\text{0}}_{{\text{i}-1}} \textbf{R}^{{\text{i}-1}}_{\text{i}}. $$

(16)

The rotations between consecutive bodies are expressed using intrinsic ZXZ-Euler angles. The rotation matrix corresponding to a specific set of angles is obtained using the following function [11]:

$$ \text{ZXZEuler}(Z_{1}, X_{2}, Z_{3}) = \begin{bmatrix} \text{c}_{1} \text{c}_{3} - \text{s}_{1} \text{c}_{2} \text{s}_{3} & - \text{c}_{1} \text{s}_{3} - \text{s}_{1} \text{c}_{2} \text{c}_{3} & \text{s}_{1} \text{s}_{3} \\ \text{s}_{1} \text{c}_{3} + \text{c}_{1} \text{c}_{2} \text{s}_{3} & \text{c}_{1} \text{c}_{2} \text{c}_{3} - \text{s}_{1} \text{s}_{3} & - \text{c}_{1} \text{s}_{2} \\ \text{s}_{2} \text{s}_{3} & \text{s}_{2} \text{c}_{3} & \text{c}_{2} \end{bmatrix} , $$

(17)

where c and s represent the cosine and sine functions, respectively, and the sub-indices 1, 2, and 3 refer to the three considered Euler angles.

Combining the joint parameters with Equation (17), the $\textbf{R}^{\text{i-1}}_{\text{i}}$ rotation matrices can be directly determined as:

$$ \textbf{R}^{\text{i-1}}_{\text{i}} = \textstyle\begin{cases} \text{ZXZEuler}(0, \vartheta _{i}, 0) & \forall JT_{\text{i}} = R \wedge JO_{\text{i}} = x, \\ \text{ZXZEuler}\left (\frac{\uppi}{2}, \vartheta _{i}, - \frac{\uppi}{2}\right ) & \forall JT_{\text{i}} = R \wedge JO_{ \text{i}} = y, \\ \text{ZXZEuler}(0, 0, \vartheta _{i}) & \forall JT_{\text{i}} = R \wedge JO_{\text{i}} = z, \\ \text{ZXZEuler}(0, 0, 0) & \forall JT_{\text{i}} = P. \end{cases} $$

(18)

This expression defines all rotations between the consecutive bodies of CHARMIE, which in turn is used to determine the orientation of all bodies, concluding step two of the kinematics recursive algorithm.

The third and final step is moving the body to its current position in the global coordinate reference. This is achieved by moving point $\text{A}_{\text{i}}$ of the current body to point $\text{B}_{\text{i}-1}$ contained in its predecessor in the kinematic chain (whose position in the global coordinates was determined in the previous iteration). This process guarantees cohesion between the joints of the assembly. By moving all $\text{P}_{\text{i}}$ points along this same path, body i is fully translated into its actual position, as described by the formulation:

$$ \textbf{P}_{\text{i}} = \textbf{P}''_{\text{i}} + (\textbf{B}_{\text{i}-1} - \textbf{A}''_{\text{i}-1}). $$

(19)

The three steps are repeated for all bodies of the main kinematic chains, determining their positions and orientations in the global reference. By concluding the recursive algorithm for the forward kinematics analysis and implementing it into the CHARMIE robot, the model shown in Fig. 6 was obtained.

Fig. 6

3D model of CHARMIE resulting from the recursive algorithm for forward kinematics. Only bodies that are a part of the main kinematic chains are represented

4.3 Kinematics of additional bodies

The recursive algorithm solves only the placement of links that are a part of the main kinematic branches. In the CHARMIE robot, there are additional components placed between bodies 3, 4, and 5 that constitute closed loops, namely linear actuators, extension springs, and the auxiliary bars of a static balancing mechanism.

In the particular case where the two ends of an auxiliary body are points incorporated within main bodies, such is the case for both linear actuators and the extensions springs between links 4 and 5, the position and orientation can be fully defined from the coordinates of points established in the recursive algorithm. The length $l_{\text{s}}$ of these auxiliary bodies is calculated with the expression:

$$ l_{\text{s}} = \sqrt{\left (S_{\text{i}}x-S_{\text{j}}x\right )^{2} + \left (S_{\text{i}}y-S_{\text{j}}y\right )^{2} + \left (S_{\text{i}}z-S_{ \text{j}}z\right )^{2}}, $$

(20)

where $\text{S}_{\text{i}}$ and $\text{S}_{\text{j}}$ are the two ends of the considered body.

The orientation is then determined using the law of cosines. A new point, $\text{R}_{\text{i}}$, is added to one of the adjacent main bodies. $\text{R}_{\text{i}}$ must be aligned along one of the local axes with the connection point $\text{S}_{\text{i}}$. Points $\text{R}_{\text{i}}$, $\text{S}_{\text{i}}$, and $\text{S}_{\text{j}}$ now form a triangle whose sides have lengths $l_{s}$, $a_{s}$ and $b_{s}$. These lengths can all be calculated using Equation (20). The rotation $\alpha _{\text{s}}$ between the chosen local axis of the main body and the corresponding local axis of the auxiliary body is then determined using the expression:

$$ \alpha _{\text{s}}= \frac{{l_{\text{s}}}^{2}+{b_{\text{s}}}^{2}-{a_{\text{s}}}^{2}}{2l_{\text{s}}b_{\text{s}}}, $$

(21)

where $a_{s}$ is the side of the triangle opposite the calculated internal angle.

The mechanism responsible for the static balancing of the squatting motion of CHARMIE, placed between links 3 and 4, contains all bodies that cannot be directly analyzed by the method described above. This mechanism is composed of two symmetrical halves, so for simplicity only the left side (bodies $4_{\text{sla}}$, $4_{\text{slb}}$, and $4_{\text{slc}}$ of Fig. 3) is studied. The coordinates of three points, $\text{B}_{\text{4sla}}$, $\text{B}_{\text{4slb}}$, and $\text{B}_{\text{4slc}}$ (see Fig. 7), must be determined to fully characterize the required bodies.

Fig. 7

Geometric parameters required for the kinematic analysis of CHARMIE’s auxiliary bodies responsible for the static balancing of the motion between links 3 and 4

At this stage, all main bodies have been modeled, so points from both links 3 and 4 can be expressed in the local coordinates of link 3. The altitude of $\text{B}_{\text{4slc}}$ can be directly determined since:

$$ B^{\prime \,\text{3}}_{\text{4slc}}z = H^{\prime \,\text{3}}_{\text{4}}z. $$

(22)

Equations (23)–(24) can then be used to determine the coordinates of $\text{B}_{\text{4sla}}$:

$$ B^{\prime \,\text{3}}_{\text{4sla}}z = B^{\prime \,\text{3}}_{\text{4slc}}z - c_{ \text{4slc,}} $$

(23)

$$ B^{\prime \,\text{3}}_{\text{4sla}}x = F^{\prime \,\text{3}}_{\text{3}}x - \sqrt{{b_{ \text{4sla}}}^{2}-\left (B^{\prime \,\text{3}}_{\text{4sla}}z - F^{\prime \,\text{3}}_{ \text{3}}z \right )^{2}}. $$

(24)

Point $\text{B}_{\text{4slc}}$ can subsequently be fully defined using:

$$ B^{\prime \,\text{3}}_{\text{4slc}}x = B^{\prime \,\text{3}}_{\text{4slc}}x. $$

(25)

The coordinates of the last point, $\text{B}_{\text{4slb}}$, are calculated using the triangle proportionality theorem as follows:

$$ B^{\prime \,\text{3}}_{\text{4slb}}x = F^{\prime \,\text{3}}_{\text{3}}x + \frac{a_{\text{4sla}}}{b_{\text{4sla}}} \left ( B^{\prime \,\text{3}}_{ \text{4sla}}x - F^{\prime \,\text{3}}_{\text{3}}x \right ), $$

(26)

$$ B^{\prime \,\text{3}}_{\text{4slb}}z = F^{\prime \,\text{3}}_{\text{3}}z + \frac{a_{\text{4sla}}}{b_{\text{4sla}}} \left ( B^{\prime \,\text{3}}_{ \text{4sla}}z - F^{\prime \,\text{3}}_{\text{3}}z \right ). $$

(27)

The points obtained from Equations (22)–(27) are expressed regarding the local reference of body 3. The global coordinates of the auxiliary bodies are determined by applying to these points the same rotation and translation that body 3 undergoes during the iterations of the recursive algorithm.

With the full definition of all auxiliary bodies, the forward kinematics analysis of CHARMIE is concluded, resulting in the model shown in Fig. 8.

Fig. 8

Complete model of CHARMIE assembled in the simulation environment as a result of the forward kinematic analysis

5 Inverse dynamics analysis

The inverse dynamics analysis processes the joint positions, velocities, and accelerations from the robot’s trajectories, the rotation matrices from the recursive kinematics algorithm, and the physical properties of the robot’s bodies and joints to compute the forces and torques applied in each pair of connected bodies.

The approach used for the study of the dynamics revolves around the recursive Newton–Euler algorithm of [7]. The used formulation is based on the algorithm presented in [4], which solves the dynamics of bodies assembled in serial chains modeled using the Denavit–Hartenberg parameters. The present work modifies this algorithm to allow automatically tackle tree structure kinematic chains, integrate other formulations to solve closed loops, and use any axis orientation (instead of limiting them due to the Denavit–Hartenberg notation). At this stage, some physical effects, such as the internal inertia of each motor, were neglected to increase computational efficiency.

By using this adapted formulation, the inverse dynamics analysis was divided into two main parts. The first is completely running the recursive algorithm for all branched components of the open kinematic chain. The latter is computing the additional formulations, which tackle the closed systems between CHARMIE’s bodies 3, 4, and 5.

5.1 Recursive algorithm for the inverse dynamics analysis of the main bodies

The recursive algorithm that studies the inverse dynamics is divided into two stages. The first stage, referred to as the forward iterations, is preparatory. Progressing from the robot’s base to the end-effectors, each iteration uses information regarding the previous body in the kinematic chain, and data defining the preceding joint, to determine the angular velocity and acceleration of the current body, as well as the linear acceleration of its key points (center of mass and joints). With the first stage concluded, the second stage, referred to as the backwards iterations, starts the calculations on the end-effectors and progresses towards the base of the robot. The algorithm uses information from the forward iterations, along with the body mass and inertia properties, to calculate the sum of forces and reactions applied on each body by its predecessors in the kinematic chain. If a body possesses only a single link preceding it, this sum automatically defines the forces and torques applied by it. In all equations in this section of the work, the superscript next to a variable denotes the coordinate reference it is expressed on.

Before beginning the algorithm, the unit vector $\textbf{z}_{\text{i}}$, characterizing the joint orientation preceding body $\text{i,}$ must be defined using Equation (28).

$$ \textbf{z}_{\text{i}} = \textstyle\begin{cases} [1, 0, 0]^{\text{T}} & \forall JO_{\text{i}} = x, \\ [0, 1, 0]^{\text{T}} & \forall JO_{\text{i}} = y, \\ [0, 0, 1]^{\text{T}} & \forall JO_{\text{i}} = z. \end{cases} $$

(28)

The forward iterations start by calculating the angular velocity $\boldsymbol{\upomega}^{\text{i}}_{\text{i}}$ of the current body with Equation (29).

$$ \boldsymbol{\upomega}^{\text{i}}_{\text{i}} = \textstyle\begin{cases} \textbf{R}^{\text{i}-1\text{T}}_{\text{i}} \boldsymbol{\upomega}^{ \text{i}-1}_{\text{i}-1} & \forall JT_{\text{i}} = \text{P,} \\ \textbf{R}^{\text{i}-1\text{T}}_{\text{i}} \left ( \boldsymbol{\upomega}^{ \text{i}-1}_{\text{i}-1} + {\dot{\vartheta}}_{\text{i}} \textbf{z}_{ \text{i}} \right ) & \forall JT_{\text{i}} = \text{R,} \end{cases} $$

(29)

where $\textbf{R}^{\text{i}-1\text{T}}_{\text{i}}$ is the rotation matrix used to convert data expressed regarding the preceding body to the reference of the current body, $\boldsymbol{\upomega}^{\text{i}-1}_{\text{i}-1}$ is the angular velocity of the preceding body in its reference, and ${\dot{\vartheta}}_{\text{i}}$ is the rotation velocity of the revolute joint preceding body i.

The angular acceleration $\dot{\boldsymbol{\upomega}}^{\text{i}}_{\text{i}}$ of body i can then be determined using:

$$ \dot{\boldsymbol{\upomega}}^{\text{i}}_{\text{i}} = \textstyle\begin{cases} \textbf{R}^{\text{i}-1\text{T}}_{\text{i}} \dot{\boldsymbol{\upomega}}^{ \text{i}-1}_{\text{i}-1} & \forall JT_{\text{i}} = \text{P,} \\ \textbf{R}^{\text{i}-1\text{T}}_{\text{i}} \left ( \dot{\boldsymbol{\upomega}}^{\text{i}-1}_{\text{i}-1} + { \ddot{\vartheta}}_{\text{i}} \textbf{z}_{\text{i}} + {\dot{\vartheta}}_{ \text{i}} \dot{\boldsymbol{\upomega}}^{\text{i}-1}_{\text{i}-1} \times \textbf{z}_{\text{i}} \right ) & \forall JT_{\text{i}} = \text{R,} \end{cases} $$

(30)

where $\dot{\boldsymbol{\upomega}}^{\text{i}-1}_{\text{i}-1}$ is the angular acceleration of the preceding body expressed in its reference axis, and ${\ddot{\vartheta}}_{\text{i}}$ the angular acceleration of the revolute joint preceding body i.

With the angular velocity and acceleration of the body determined, it is then possible to calculate the linear acceleration of its key points. These accelerations must include the Coriolis, Euler, and centrifugal effects for the posterior force and torque calculations. The $\ddot{\textbf{p}}^{\text{i}}_{\text{Bi}}$ linear acceleration of the $\text{B}_{\text{i}}$ points connecting body i to its succeeding bodies are computed as:

$$ \ddot{\textbf{p}}^{\text{i}}_{\text{Bi}} = \textstyle\begin{cases} \begin{aligned} & \textbf{R}^{\text{i}-1\text{T}}_{\text{i}} \left ( \ddot{\textbf{p}}^{ \text{i}-1}_{\text{Bi}-1} + \ddot{d}_{\text{i}} \textbf{z}_{\text{i}} \right ) + \dot{\boldsymbol{\upomega}}^{\text{i}}_{\text{i}} \times \textbf{r}^{\text{i}}_{\text{Ai,Bi}} \\ & + \boldsymbol{\upomega}^{\text{i}}_{\text{i}} \times \left ( \boldsymbol{\upomega}^{\text{i}}_{\text{i}} \times \textbf{r}^{\text{i}}_{ \text{Ai,Bi}} \right ) + 2 \dot{d}_{\text{i}} \boldsymbol{\upomega}^{ \text{i}}_{\text{i}} \times \textbf{R}^{\text{i}-1\text{T}}_{\text{i}} \textbf{z}_{\text{i}} \end{aligned} & \forall JT_{\text{i}} = \text{P,} \\ \textbf{R}^{\text{i}-1\text{T}}_{\text{i}} \ddot{\textbf{p}}^{\text{i}-1}_{ \text{Bi}-1} + \dot{\boldsymbol{\upomega}}^{\text{i}}_{\text{i}} \times \textbf{r}^{\text{i}}_{\text{Ai,Bi}} + \boldsymbol{\upomega}^{\text{i}}_{ \text{i}} \times \left (\boldsymbol{\upomega}^{\text{i}}_{\text{i}} \times \textbf{r}^{\text{i}}_{\text{Ai,Bi}} \right ) \ & \forall JT_{\text{i}} = \text{R,} \end{cases} $$

(31)

where $\ddot{\textbf{p}}^{\text{i}-1}_{\text{Bi}-1}$ is the linear acceleration of point $\text{A}_{\text{i}}$ expressed in the reference of the preceding body, $\dot{d}_{\text{i}}$ and $\ddot{d}_{\text{i}}$ are the velocity and acceleration of the linear actuator preceding body i, and $\textbf{r}^{\text{i}}_{\text{Ai,Bi}}$ is the vector pointing from the preceding joint $\text{A}_{\text{i}}$ to the succeeding joint $\text{B}_{\text{i}}$.

The forward iterations of the recursive algorithm are finished by determining the linear acceleration of the center of mass $\ddot{\textbf{p}}^{\text{i}}_{\text{Ci}}$ using the expression:

$$ \ddot{\textbf{p}}^{\text{i}}_{\text{Ci}} = \ddot{\textbf{p}}^{\text{i}}_{ \text{Bi}} + \dot{\boldsymbol{\upomega}}^{\text{i}}_{\text{i}} \times \left ( \textbf{r}^{\text{i}}_{\text{Ai,Ci}} - \textbf{r}^{\text{i}}_{ \text{Ai,Bi}} \right ) + \boldsymbol{\upomega}^{\text{i}}_{\text{i}} \times \left (\boldsymbol{\upomega}^{\text{i}}_{\text{i}} \times \left ( \textbf{r}^{\text{i}}_{\text{Ai,Ci}} - \textbf{r}^{\text{i}}_{\text{Ai,Bi}} \right ) \right ), $$

(32)

where $\textbf{r}^{\text{i}}_{\text{Ai,Ci}}$ is the vector pointing from the preceding joint $\text{A}_{\text{i}}$ to the center of mass $\text{C}_{\text{i}}$.

The backwards iterations commence by first determining the sum of the $\textbf{f}\,^{\text{i}}_{\text{j}_{\text{a}}}$ forces applied by the $n_{a}$ preceding bodies on the current body i using the formulation:

$$ \sum _{\text{j}_{\text{a}}=1}^{n_{a}} \textbf{f}\,^{\text{i}}_{\text{j}_{ \text{a}}} = m_{\text{i}} \ddot{\textbf{p}}^{\text{i}}_{\text{Ci}} + \sum _{\text{j}_{\text{b}}=1}^{n_{b}} \left ( \textbf{R}^{\text{i}}_{ \text{j}_{\text{b}}} \textbf{f}\,^{\text{j}_{\text{b}}}_{\text{j}_{\text{b}}} \right ), $$

(33)

where $\text{j}_{\text{a}}$ is the pointer selecting each preceding body for the sum, $n_{b}$ is the number of succeeding bodies, $\text{j}_{\text{b}}$ the pointer for each succeeding body, $m_{\text{i}}$ the mass of the current body being studied, $\textbf{f}\,^{\text{j}_{\text{b}}}_{\text{j}_{\text{b}}}$ the force applied by the current body on the ${\text{j}_{\text{b}}}$ succeeding body, expressed in the coordinates of ${\text{j}_{\text{b}}}$, and $\textbf{R}^{\text{i}}_{\text{j}_{\text{b}}}$ the rotation matrix used for converting the local reference of each body ${\text{j}_{\text{b}}}$ into the reference of body i.

The sum of the $\boldsymbol{\upmu}\,^{\text{i}}_{\text{j}_{\text{a}}}$ torques applied by the $n_{a}$ preceding bodies is then determined by the expression:

$$ \begin{aligned} \sum _{\text{j}_{\text{a}}=1}^{n_{a}} \boldsymbol{\upmu}\,^{ \text{i}}_{\text{j}_{\text{a}}} = \ & \bar{\textbf{I}}_{\text{i}}^{ \text{i}} \dot{\boldsymbol{\upomega}}_{\text{i}}^{\text{i}} + \boldsymbol{\upomega}_{\text{i}}^{\text{i}} \times \left ( \bar{\textbf{I}}_{\text{i}}^{\text{i}} \boldsymbol{\upomega}_{\text{i}}^{ \text{i}} \right ) + \sum _{{\text{j}_{\text{a}}}=1}^{n_{a}} \left (- \textbf{f}\,^{\text{i}}_{\text{j}_{\text{a}}} \times \textbf{r}^{\text{i}}_{ \text{Aja,Ci}}\right ) \\ & + \sum _{\text{jb}=1}^{n_{b}} \left ( \textbf{R}^{\text{i}}_{\text{j}_{ \text{b}}} \textbf{f}\,^{\text{j}_{\text{b}}}_{\text{j}_{\text{b}}} \times \left ( \textbf{r}^{\text{i}}_{\text{Ai,Ci}} -\textbf{r}^{\text{i}}_{ \text{Ai,Bjb}} \right ) + \textbf{R}^{\text{i}}_{\text{j}_{\text{b}}} \boldsymbol{\upmu}^{\text{j}_{\text{b}}}_{\text{j}_{\text{b}}} \right ), \end{aligned} $$

(34)

where $\bar{\textbf{I}}_{\text{i}}^{\text{i}}$ is the inertia matrix of body i, $\textbf{r}^{\text{i}}_{\text{Ai,Bjb}}$ the vector pointing from joint $\text{A}_{\text{i}}$ to the joint with link ${\text{j}_{\text{b}}}$, and $\boldsymbol{\upmu}^{\text{j}_{\text{b}}}_{\text{j}_{\text{b}}}$ the torque applied by the current body i on the succeding body ${\text{j}_{\text{b}}}$, expressed in the coordinates of ${\text{j}_{\text{b}}}$.

When applying this algorithm to the CHARMIE robot, the forward iterations can be completed for all of the links. However, since link 5 is preceded by a closed and overconstrained loop, the backwards iterations must stop upon reaching this body. To fully characterize the dynamics of links 4 and 5, additional formulations are required that define how the obtained sum of forces and torques is distributed along each preceding body.

5.2 Analysis of the closed and overconstrained loops

The multibody dynamics analysis of overconstrained systems is a complex and continuously discussed problem in the literature. Several high-fidelity methods have been presented for dealing with these systems, such as in [35, 36]. In the study of CHARMIE, the simplicity of the mechanisms, and the level of accuracy required from the results, allow some simplifications that permit a direct algebraic approach, solving these systems with minimal computational resources.

This section tackles the force and torque distribution in two of CHARMIE’s links, body 4 and body 5 (see Fig. 9). Since this study is a part of the backwards iterations of the recursive algorithm, body 5 must be tackled first.

Fig. 9

Free-body diagram of CHARMIE’s (a) body 5, and (b) body 4. The known forces and torques at the beginning of the study of each body are colored in green hues, while unknown variables to be calculated are colored in orange hues (Color figure online)

Figure 9a shows the three links applying unknown forces and torques on body 5, namely the linear actuator $\text{5}_{\text{d}}$, the extension spring $\text{5}_{\text{s}}$, and the preceding link (body 4). The forces must be studied first, since they influence the balance of torques.

The extension spring only applies a tangential force, which depends solely on its extension and orientation. The intensity $F_{\text{5s}}$ of this force is determined by Equation (35).

$$ F_{\text{5s}} = k_{s} (L_{\text{0}} - L_{\text{s}}) + F_{0}\text{s}, $$

(35)

where $k_{s}$ is the spring constant, $L_{\text{0}}$ the spring’s free length, $L_{\text{s}}$ the spring’s current length and $F_{0}\text{s}$ the force originating from the pretension applied during the spring’s manufacturing. The current spring length $L_{\text{s}}$ and the orientation of the force can be determined using Equations (20)–(21).

From this point onward, to simplify the expressions, the force applied by spring $\text{5}_{\text{s}}$ is treated similarly to those applied by succeeding bodies. To obtain the remaining forces, the following three conditions were taken into account:

• Since the joints of both the linear actuator and body 4 are revolute around the $x$-axis, their applied forces must guarantee the $x$ torque equilibrium.

• The force applied by the linear actuator in the $yz$-plane is tangential to the orientation of the actuator.

• Due to the similar construction of both joints, it was estimated that the linear actuator and body 4 apply half of the force reaction on the $x$-axis.

By equating these three conditions, and combining them with the sum of forces determined by Equation (33), Equation (36) was obtained.

$$ \begin{bmatrix} a_{1} \\ a_{2} \\ a_{3} \\ a_{4} \\ 0 \\ 0 \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 & 1 & 0 & 0 \\ 0 & 1 & 0 & 0 & 1 & 0 \\ 0 & 0 & 1 & 0 & 0 & 1 \\ 0 & \textbf{r}^{5}_{\text{A5d,C5}}\hat{\textbf{z}} & - \textbf{r}^{5}_{ \text{A5d,C5}}\hat{\textbf{y}} & 0 & \textbf{r}^{5}_{\text{A4,C5}} \hat{\textbf{z}} & - \textbf{r}^{5}_{\text{A4,C5}}\hat{\textbf{y}} \\ 0 & \text{cos}(\alpha _{\text{5d}}) & - \text{sin}(\alpha _{\text{5d}}) & 0 & 0 & 0 \\ -1 & 0 & 0 & 1 & 0 & 0 \end{bmatrix} \begin{bmatrix} \textbf{f}\,^{5}_{\text{5d}}\hat{\textbf{x}} \\ \textbf{f}^{5}_{\text{5d}}\hat{\textbf{y}} \\ \textbf{f}\,^{5}_{\text{5d}}\hat{\textbf{z}} \\ \textbf{f}\,^{5}_{\text{4}}\hat{\textbf{x}} \\ \textbf{f}^{5}_{\text{4}}\hat{\textbf{y}} \\ \textbf{f}\,^{5}_{\text{4}}\hat{\textbf{z}} \end{bmatrix} , $$

(36)

where $\hat{\textbf{x}}$, $\hat{\textbf{y}}$, and $\hat{\textbf{z}}$ are unit vectors related to the three axes, $\textbf{r}^{5}_{\text{A5d,C5}}$ is the vector representing the displacement from the joint of the linear actuator $\text{A}_{\text{5d}}$ to the center of mass $\text{C}_{\text{5}}$, $\textbf{r}^{5}_{\text{A4,C5}}$ is the vector representing the displacement from the joint with body 4, $\text{A}_{\text{4}}$, to the center of mass $\text{C}_{\text{5}}$, and $\alpha _{\text{5d}}$ is the angle defining the rotation of the linear actuator in relation to body 5 around the $x_{5}$-axis. The left side of the equation system is defined by the variables:

$$ \begin{aligned} a_{1} & = \sum _{{\text{j}_{\text{a}}}=1}^{2} \textbf{f}\,^{5}_{{{ \text{j}_{\text{a}}}}}\hat{\textbf{x}} , \qquad a_{2}= \sum _{{\text{j}_{ \text{a}}}=1}^{2} \textbf{f}\,^{5}_{{\text{j}_{\text{a}}}} \hat{\textbf{y}} , \qquad a_{3}=\sum _{{\text{j}_{\text{a}}}=1}^{2} \textbf{f}\,^{5}_{{\text{j}_{\text{a}}}}\hat{\textbf{z}}, \\ a_{4} & = \bar{\textbf{I}}_{5}^{5} \dot{\boldsymbol{\upomega}}_{5}^{5} \hat{\textbf{x}} +\boldsymbol{\upomega}_{5}^{5} \times \left ( \bar{\textbf{I}}_{5}^{5} \boldsymbol{\upomega}_{5}^{5} \right ) \hat{\textbf{x}} - \left (\textbf{f}\,^{5}_{\text{5s}} \times \textbf{r}^{5}_{\text{A5s,C5}}\right ) \hat{\textbf{x}} \\ & + \sum _{{\text{j}_{\text{b}}}=1}^{4} \left [ \textbf{R}^{\text{5}}_{{ \text{j}_{\text{b}}}} \textbf{f}\,^{{\text{j}_{\text{b}}}}_{{\text{j}_{ \text{b}}}} \times \left ( \textbf{r}^{5}_{\text{A5,C5}} -\textbf{r}^{5}_{ \text{A5,Bjb}} \right ) + \textbf{R}^{\text{5}}_{{\text{j}_{\text{b}}}} \boldsymbol{\upmu}^{{\text{j}_{\text{b}}}}_{{\text{j}_{\text{b}}}} \right ] \hat{\textbf{x},} \end{aligned} $$

(37)

where $a_{1}$, $a_{2}$, and $a_{3}$ represent the sum of the two unknown forces in the $x$, $y$, and $z$-axis calculated by using Equation (33) (including the spring force in the right side of the equation), and $a_{4}$ is the sum of torques of the system around the $x$-axis, ignoring the forces from the linear actuator and body 4.

Solving this equation system fully defines all forces applied in body 5. To calculate the unknown torques, one simplification was considered:

• Since the joints of the linear actuator and body 4 are similar, it was estimated that each applies half of the reaction torques around the $y$ and $z$ axes.

This simplification is expressed in two equations which, alongside the sum of torques around the $y$- and $z$-axis determined with Equation (34), result in the following formulation:

$$ \begin{bmatrix} \sum _{\text{ja}=1}^{2} \boldsymbol{\upmu}^{5}_{\text{ja}} \hat{\textbf{y}} \\ \sum _{\text{ja}=1}^{2} \boldsymbol{\upmu}^{5}_{\text{ja}} \hat{\textbf{z}} \\ 0 \\ 0 \end{bmatrix} = \begin{bmatrix} 1 & 0 & 1 & 0 \\ 0 & 1 & 0 & 1 \\ 1 & 0 & -1 & 0 \\ 0 & 1 & 0 & -1 \end{bmatrix} \begin{bmatrix} \boldsymbol{\upmu}^{5}_{\text{5d}}\hat{\textbf{y}} \\ \boldsymbol{\upmu}^{5}_{\text{5d}}\hat{\textbf{z}} \\ \boldsymbol{\upmu}^{5}_{\text{4}}\hat{\textbf{y}} \\ \boldsymbol{\upmu}^{5}_{\text{4}}\hat{\textbf{z}} \end{bmatrix}. $$

(38)

By solving these systems of equations, the distribution of forces and torques for all bodies connected to link 5 is fully characterized.

Body 4 of CHARMIE has four sets of unknown forces and torques applied to it (see Fig. 9b), which are exerted by the linear actuator $\text{4}_{\text{d}}$, the symmetric components of the static balancing mechanisms $\text{4}_{\text{src}}$ and $\text{4}_{\text{slc}}$, and the preceding link (body 3).

The dynamics of body 4 were analyzed by first independently addressing the static balancing mechanism (see Fig. 10). This study is possible since this mechanism’s forces depend only on the geometrical configuration of its spring and bars. Only the right side of the mechanism is illustrated, but the presented expressions are valid for both halves (the change in the direction of vectors automatically adjusts the equation for the respective side).

Fig. 10

Free-body diagram of CHARMIE’s body $4_{\text{sra}}$ to study the forces applied by the static balancing system between links 3 and 4

Force $F_{\text{4srb}}$ is applied by an extension spring. The tangential component of this force is calculated using Equations (20) and (35). In turn, the rotation $\alpha _{4}$ of the spring around the $y$-axis in relation to the reference of the body 4 is obtained using Equation (21). It is now possible to express the force applied by the spring in body $4_{\text{sra}}$ as:

$$ \textbf{f}\,^{\text{4}}_{\text{4sb}} = \begin{bmatrix} F_{\text{T4sb}}\text{sin}(\alpha _{4}),& 0, & F_{\text{T4s}}\text{cos}( \alpha _{4}) \end{bmatrix} , $$

(39)

where $F_{\text{T4sb}}$ is the tangential force applied by the extension spring.

The unknown forces in the static balancing system can now all be determined from the balance of forces in the $x$- and $z$-axes, together with the balance of torques around the $y$-axis. It should be noted that accelerations are not relevant for this balance, since auxiliary bodies were simplified as being massless. These considerations result in the following formulation:

$$ \begin{bmatrix} \textbf{f}\,^{\text{4}}_{\text{4srb}}\hat{\textbf{x}} \\ \textbf{f}\,^{\text{4}}_{\text{4srb}}\hat{\textbf{z}} \\ a_{5} \end{bmatrix} = \begin{bmatrix} 0 & 1 & 0 \\ - 1 & 0 & 1 \\ - \textbf{r}^{\text{4}}_{\text{A4sra,B4sra}}\hat{\textbf{x}} & 0 & 0 \end{bmatrix} \begin{bmatrix} \textbf{f}\,^{\text{4}}_{\text{4src}}\hat{\textbf{z}} \\ \textbf{f}\,^{\text{4}}_{\text{3sra}}\hat{\textbf{x}} \\ \textbf{f}\,^{\text{4}}_{\text{3sra}}\hat{\textbf{z}} \end{bmatrix} , $$

(40)

where $\textbf{r}^{\text{4}}_{\text{A4sra,B4sra}}$ is the vector representing the displacement between the connecting points of body $4_{\text{sra}}$ with links 3 and 4. The variable $a_{5}$ on the left side of Equation (40) is defined as:

$$ a_{5} = \left (\textbf{f}\,^{\text{3}}_{\text{4srb}}\hat{\textbf{z}} \right ) \left (\textbf{r}^{\text{3}}_{\text{A4sra,B4srb}} \hat{\textbf{x}} \right ) - \left (\textbf{f}\,^{\text{3}}_{\text{4srb}} \hat{\textbf{x}}\right ) \left (\textbf{r}^{\text{3}}_{ \text{A4sra,B4srb}}\hat{\textbf{z}}\right ), $$

(41)

where $\textbf{r}^{\text{3}}_{\text{A4sra,B4srb}}$ is the vector representing the displacement between the connecting points of body $4_{\text{sra}}$ with link 3 and the extension spring $4_{\text{srb}}$.

Regarding the study of torques, body $\text{4}_{\text{sra}}$ only transmits the torque applied to it around the $z$ axis directly from link 4 into link 3.

With the analysis of the static balancing mechanism concluded, the remaining forces and torques applied on body 4 can be approached. The following simplifications were taken into account:

• Actuator $\text{4}_{\text{d}}$ applies a tangential force in the $yz$-plane aligned with the orientation of the actuator.

• The joint between bodies 4 and 3 possesses a much tighter tolerance than the static balancing mechanism, so it was estimated that this joint applies all undetermined reaction forces in the $x$- and $y$-axes.

This set of simplifications allows determining the currently unknown forces applied by both the linear actuator $\text{4}_{\text{d}}$, and body 3, using the formulation:

$$ \begin{bmatrix} \sum _{\text{ja}=1}^{2} \textbf{f}\,^{4}_{\text{ja}}\hat{\textbf{x}} \\ \sum _{\text{ja}=1}^{2} \textbf{f}\,^{4}_{\text{ja}}\hat{\textbf{y}} \\ \sum _{\text{ja}=1}^{2} \textbf{f}\,^{4}_{\text{ja}}\hat{\textbf{z}} \\ 0 \end{bmatrix} = \begin{bmatrix} 0 & 0 & 1 & 0 \\ 1 & 0 & 0 & 1 \\ 0 & 1 & 0 & 0 \\ \text{cos}(\alpha _{\text{4d}}) & - \text{sin}(\alpha _{\text{4d}}) & 0 & 0 \end{bmatrix} \begin{bmatrix} \textbf{f}\,^{4}_{\text{4d}}\hat{\textbf{y}} \\ \textbf{f}^{4}_{\text{4d}}\hat{\textbf{z}} \\ \textbf{f}\,^{4}_{\text{3}}\hat{\textbf{x}} \\ \textbf{f}\,^{4}_{\text{3}}\hat{\textbf{y}} \end{bmatrix} , $$

(42)

where the sum of unknown forces is obtained using Equation (33) (considering the static balancing forces on the right side of the equation), and $\alpha _{\text{4d}}$ is the angle between the linear actuator and body 4 around the $x_{4}$-axis.

It is then possible to study the unknown torques applied in body 4. The following simplifications were taken into account:

• The joint between bodies 4 and 3 possesses a tighter tolerance than the static balancing mechanism and linear actuator, so it was estimated that this joint applies all undetermined reaction torques in the $x$- and $y$-axes.

• Since the joint with body 3 cannot apply any torque around the $z$-axis, and the static balancing mechanism joints are more rigid than the linear actuator ones, it was estimated that each of these joints applies half of the torque reaction in the $z$-axis.

These conditions result in the following system of equations:

$$ \begin{bmatrix} \sum _{\text{ja}=1}^{2} \boldsymbol{\upmu}^{4}_{\text{ja}} \hat{\textbf{x}} \\ \sum _{\text{ja}=1}^{2} \boldsymbol{\upmu}^{4}_{\text{ja}} \hat{\textbf{y}} \\ \sum _{\text{ja}=1}^{2} \boldsymbol{\upmu}^{4}_{\text{ja}} \hat{\textbf{z}} \\ 0 \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 1 \\ 0 & 0 & 1 & -1 \end{bmatrix} \begin{bmatrix} \boldsymbol{\upmu}^{4}_{\text{3}}\hat{\textbf{x}} \\ \boldsymbol{\upmu}^{4}_{\text{3}}\hat{\textbf{y}} \\ \boldsymbol{\upmu}^{4}_{\text{3slr}}\hat{\textbf{z}} \\ \boldsymbol{\upmu}^{4}_{\text{3slc}}\hat{\textbf{z}} \end{bmatrix} , $$

(43)

where the sum of torques are determined using Equation (34). With this analysis, the forces and torques applied to link 4 are determined.

With the distribution of all forces and torques applied on bodies 4 and 5 established, the recursive algorithm can conclude its iterations for the remaining bodies, finishing the full dynamic analysis of the CHARMIE robot.

6 Results and discussion

The algorithms and mathematical equations described in this work were implemented into Python. The numpy library was used to assist in mathematical and algebraic operations, and the matplotlib library allowed the generation of graphics showing data and 3D plots representing the robot’s motion. The resulting code is hardware independent and can be implemented into the robot’s embedded computer.

Four different models were prepared to evaluate the algorithm’s computational efficiency: a 2-DOF Arm, a 4-DOF Arm, a 6-DOF Arm and the CHARMIE Robot. These models were simulated in PyCharm on a computer with an AMD Ryzen 5 5600X 6-Core Processor 3.70 GHz. Each model was simulated by analysing either only the kinematic or both kinematic and dynamic properties. The animation of the motion was activated and deactivated to evaluate its computational weight, producing the results shown in Table 5.

Table 5 Comparison of the average computational time required for 1000 timesteps of simulation (including the code initialization) for different models under the same conditions. Studies a) were conducted with no graphical interface, while studies b) illustrated the robot’s motion over time

The computational time of both the kinematic and dynamic formulations increases linearly with the number of bodies of the tested robotic arms, as expected from a recursive algorithm. This linearity cannot be extrapolated directly for the CHARMIE robot, since this analysis must also tackle closed kinematic loops. The animation of the model’s motion, required for validation and testing by the researcher but not for employing the simulator in its desired applications, consumes the greatest amount of computational resources when enabled, utilizing 99% of the computational time. These results also show that computational times can be reduced from 60% to 70% (the bigger the model, the greater the reduction) by disabling the inverse dynamics analysis.

The results from CHARMIE’s kinematics and dynamics analyzes were validated by comparing them with those obtained from a commercial software (Visual Nastran 4D). Figure 11 shows a side-by-side comparison of both studied models. Two main differences exist between the two simulations: the first is that in the commercial software, the robot was simplified by removing the closed and overconstrained loops; the second is that the commercial software calculates the inertia matrices directly from the primitive shapes and their associated mass, therefore, these estimated inertia matrices differ from those implemented in the recursive algorithm calculated from CAD models with greater detail.

Fig. 11

Multibody model of the CHARMIE robot in two different computational environments: (a) in a commercial software (Visual Nastran 4D) for the time frame t = 0s, (b) in the developed methodology for the time frame t = 1.3 s with red arrows representing the main directions of motion (Color figure online)

The same 5-second motion was implemented in both systems to allow a direct comparison of results. The goal of the chosen trajectory was not to be realistic (it includes impossible configurations in a physical prototype, such as penetrating bodies), but to be simple to compute and analyze. The movement starts from a reference position (Fig. 11a), where all joints are in their 0 positions (except for the two linear actuators in the robot’s body, with an initial total extension defined as 400 mm). A reference acceleration was then associated with all joints: every revolute joint (including the one defining the robot’s yaw) has an angular acceleration of $\uppi /9$ rad/$\text{s}^{2}$, the two linear actuators in the robot’s body have an acceleration of 3 mm/$\text{s}^{2}$, and the two linear actuators controlling the robot’s locomotion have an acceleration of 30 mm/$\text{s}^{2}$. The acceleration defined over time for each actuator in the simulations corresponds to the aforementioned reference accelerations multiplied by a sign function, which assumes a negative value for the first 2.5 seconds of motion and a positive one for the last 2.5 seconds. The robot is fully stopped after its movement is concluded.

When implementing a recursive algorithm, any error propagates along the iterative calculations. The part of the algorithm which defines the kinematics begins its analysis on the robot’s base, and finishes in the end-effectors, therefore, the best point for evaluation is a point contained in the end-effectors. For this effect, point $\text{D}_{\text{12a}}$ was added to body 12a with the local coordinates of $[0, 0, 0.1023]\text{ m}$ which corresponds to the center of the end-effector claw (this point is highlighted with an orange sphere in Fig. 11). Accelerations, velocities, positions, and orientations are all interconnected, and mistakes in the velocities or accelerations would reflect in the results of the dynamic calculations, therefore, to validate the kinematic model, it is sufficient to evaluate the position of this point over time. The results from Fig. 12 show a complete overlap between the outcomes of both methods. A visual inspection of both simulations also showed identical behavior.

Fig. 12

Positions of the same point in body 12a (the robot’s left claw) obtained from the forward kinematics analysis using both a commercial software, and the methodology of the present study

The dynamics were studied by applying the same principle of error propagation as for the kinematics analysis. The comparison focuses on the forces applied by the floor plane on the robot’s base which, considering the simplifications made to the locomotion system, corresponds to the loads applied by body 2 on body 3. The alterations made to the robot’s configuration in the commercial software (to remove over-constrained loops) cause minimal changes in these results, since they only affect how the forces propagate within the robot, not how it interacts with the floor plane. The two models are similar, yet not identical, so their dynamic properties regarding the generated torques cannot be compared directly. The juxtaposition of results in Fig. 13 shows that both models produced identical results.

Fig. 13

Forces applied by body 2 on body 3 (the robot’s base) obtained from the inverse dynamic analysis using a commercial software and the methodology of the present study

With two different sources providing identical results for the system’s analysis, the implemented recursive algorithms were considered validated, as well as the seven-step methodology used for the CHARMIE project.

7 Concluding remarks

This study presents a novel seven-step methodology for the multibody analysis of complex articulated robotic systems. This formulation is centred around two recursive algorithms, one for the forward kinematic analysis, and the other to model the inverse dynamics, which are well known from the literature. The methodology is focused on developing models tailored for the system being analyzed, resulting in higher computational efficiency despite a loss in the generalizability. One of the main advantages of the used method is its modularity, allowing different mathematical notations to be used, as well as permitting an easy integration of other formulations to model more specific physical phenomena (such as applying external loads or simulating the locomotion using forward dynamics).

The seven-step methodology was further detailed by using it to successfully create the multibody model of the CHARMIE mobile manipulator. The robot’s relative complexity illustrates the flexibility of the used method. This same robot was also modeled in a commercial software to confirm and validate the produced outcomes.

The obtained model is ideal for the current required analyses of CHARMIE. Nonetheless, a set of future works has been identified to characterize, improve and further validate the presented methodology. The computational efficiency of the used method can be further specified by calculating the number of mathematical operations required from each module of the program. This process should be accompanied by an optimization of the implementation of the current code in Python to maximize its computational efficiency. Separate formulations should be tested and compared within the modular structure of the seven-step methodology; besides increasing the computational efficiency, this may also improve the generability of the methodology without compromising its strengths. To further validate the results and the applicability of the developed simulator, it will be implemented on the embedded controller of the CHARMIE robot; the behavior from the physical model will be compared with the predictions generated by the mathematical model.

With a finished and validated own-code solution for analysing CHARMIE, the results are now playing a pivotal role in a set of ongoing studies for the robot’s development. The ability to alter geometries and properties using a single variable, which then updates the entire kinematics and dynamics accordingly, is being employed to generate quick automatic parametric optimizations. The current applications of this method include determining the required actuator torques for the arms, optimizing the static balancing mechanism associated with the linear actuators, and training a neural-network solution to control the robot trajectories.