Kinematic Modelling and Motion Analysis of a Humanoid Torso Mechanism

Featured Application: Model-based motion planning of cable-driven compliant mechanisms with a ﬂexible backbone. Abstract: This paper introduces a novel kinematic model for a tendon-driven compliant torso mechanism for humanoid robots, which describes the complex behaviour of a system characterised by the interaction of a complex compliant element with rigid bodies and actuation tendons. Inspired by a human spine, the proposed mechanism is based on a ﬂexible backbone whose shape is controlled by two pairs of antagonistic tendons. First, the structure is analysed to identify the main modes of motion. Then, a constant curvature kinematic model is extended to describe the behaviour of the torso mechanism under examination, which includes axial elongation/compression and torsion in addition to the main bending motion. A linearised stiffness model is also formulated to estimate the static response of the backbone. The novel model is used to evaluate the workspace of an example mechanical design, and then it is mapped onto a controller to validate the results with an experimental test on a prototype. By replacing a previous approximated model calibrated on experimental data, this kinematic model improves the accuracy and efﬁciency of the torso mechanism and enables the performance evaluation of the robot over the reachable workspace, to ensure that the tendon-driven architecture operates within its wrench-closure workspace.


Introduction
Mobile robots are often limited in their tasks by an environment designed to be inhabited by humans. A wheeled robot, for example, cannot climb stairs, while an animallike robot, such as hexapods or quadrupeds, is often stopped by a closed door. Conversely, humanoid robots can interact with human-sized items and navigating in human-sized environments. Furthermore, people tend to react better to humanoid robots than to other architectures [1].
Most humanoid robots are developed to optimise their limb mobility with a "black box" approach, in which the locomotion system is developed independently from the manipulation system and then integrated only through control software. Thus, complex leg and arm designs can be found in the literature [2], whereas the robot torso usually consists of a single body with no mobility. This kind of architecture can be observed in the most successful humanoid robots, including Honda's ASIMO [3], University of Waseda's WABIAN family [4], and Softbank's NAO [5] and Pepper [6].
However, the human torso plays a key role in both locomotion and manipulation tasks, by supporting dynamic balance and increasing reach and dexterity [7][8][9]. Thus, recent lightweight 3D-printed frame [17]. Its torso mechanism, CAUTO (CAssino hUmanoid TOrso), is bioinspired, reproducing Functional Spine Units (FSU). The torso is at the core of the LARMbot performance, as it contributes to both walking balance, general navigation, and manipulation tasks thanks to the additional three active DoFs that it provides to the system. The main geometrical parameters of this mechanism are reported in Table 1, while the main components are illustrated on a 3D-printed prototype in Figure 2.
The main structural element of the torso is the compliant backbone, made of commercial flexible shaft couplers that are characterised by a compliant response to bending and a stiffer response to axial torsion, compression, and elongation. As these components connect the rigid vertebrae of the spine, they determine the mobility of the system. The intrinsic stiffness of the backbone also dampens the effect of unexpected wrenches on the system and restores a straight torso position in case of structural failure of the tendons.  [17,18]: (a) Architecture and mobility; (b) A prototype. Table 1. Technical specifications of the LARMbot's torso mechanism (CAUTO) [15,16].

Width [mm]
Depth lightweight 3D-printed frame [17]. Its torso mechanism, CAUTO (CAssino hUmanoid TOrso), is bioinspired, reproducing Functional Spine Units (FSU). The torso is at the core of the LARMbot performance, as it contributes to both walking balance, general navigation, and manipulation tasks thanks to the additional three active DoFs that it provides to the system. The main geometrical parameters of this mechanism are reported in Table 1, while the main components are illustrated on a 3D-printed prototype in Figure 2.
The main structural element of the torso is the compliant backbone, made of commercial flexible shaft couplers that are characterised by a compliant response to bending and a stiffer response to axial torsion, compression, and elongation. As these components connect the rigid vertebrae of the spine, they determine the mobility of the system. The intrinsic stiffness of the backbone also dampens the effect of unexpected wrenches on the Figure 2. A prototype of the LARMbot's torso mechanism with its main components [15,16]. The main structural element of the torso is the compliant backbone, made of commercial flexible shaft couplers that are characterised by a compliant response to bending and a stiffer response to axial torsion, compression, and elongation. As these components connect the rigid vertebrae of the spine, they determine the mobility of the system. The intrinsic stiffness of the backbone also dampens the effect of unexpected wrenches on the system and restores a straight torso position in case of structural failure of the tendons. The configuration of the couplers is actuated by four tendons that determine the pose of the upper torso with respect to the lower torso.

Kinematic Modelling of a Compliant Tendon-Driven Torso Mechanism
When compared to other similar tendon-driven mechanisms with four cables driving a joint, such as the rehabilitation device in [20], the LARMbot's torso mechanism poses additional challenges due to the compliance of the backbone. First, the backbone is not an idle joint, and, because of its stiffness, it exerts a wrench on the upper platform in any non-straight configuration. This wrench results in a higher actuation force required to move the torso mechanism but improves the dynamic behaviour of the mechanism by restoring a stable position when force closure is not achieved [20]. Furthermore, the torso mechanism, which is illustrated in the kinematic scheme in Figure 3, is also characterised by a degree of underactuation.
Appl. Sci. 2021, 11, x FOR PEER REVIEW 4 of The configuration of the couplers is actuated by four tendons that determine the pose the upper torso with respect to the lower torso.  1 Bending angle, direction of bending, axial torsion, axial elongation. 2 The tendon-driven architec ture means that four motors can actively control three degrees of freedom.

Kinematic Modelling of a Compliant Tendon-Driven Torso Mechanism
When compared to other similar tendon-driven mechanisms with four cables drivin a joint, such as the rehabilitation device in [20], the LARMbot's torso mechanism pose additional challenges due to the compliance of the backbone. First, the backbone is not a idle joint, and, because of its stiffness, it exerts a wrench on the upper platform in any non straight configuration. This wrench results in a higher actuation force required to mov the torso mechanism but improves the dynamic behaviour of the mechanism by restorin a stable position when force closure is not achieved [20]. Furthermore, the torso mech nism, which is illustrated in the kinematic scheme in Figure 3, is also characterised by degree of underactuation.  The backbone has four DoFs, which can be characterised by the following paramete with reference to the kinematic schemes in Figure 3: • Backbone bending angle , which is defined as the angle between the x0y0 plane the lower torso frame {S0} and the xy plane of the upper torso frame {S}. • Direction of bending , which is defined as the angle between the plane of bendin and the x0y0 plane of the lower torso frame {S0}. • Axial torsion ∆ , which is defined as the angle between the x0z0 plane of the low torso frame {S0} and the xz plane of the upper torso frame {S}.

•
Axial elongation/compression ∆ℓ, which is defined as the variation of the neutr backbone length ℓ caused by the wrench acting on the component.
However, only three of these DoFs can be actively controlled by the four actuator as the tendons (routed as per Figure 3b,c) can only pull, but not push [20]. Thus, the con The backbone has four DoFs, which can be characterised by the following parameters with reference to the kinematic schemes in Figure 3: • Backbone bending angle θ, which is defined as the angle between the x 0 y 0 plane of the lower torso frame {S 0 } and the xy plane of the upper torso frame {S}. • Direction of bending ϕ, which is defined as the angle between the plane of bending and the x 0 y 0 plane of the lower torso frame {S 0 }. • Axial torsion ∆ϕ, which is defined as the angle between the x 0 z 0 plane of the lower torso frame {S 0 } and the xz plane of the upper torso frame {S}. • Axial elongation/compression ∆ , which is defined as the variation of the neutral backbone length caused by the wrench acting on the component.
However, only three of these DoFs can be actively controlled by the four actuators, as the tendons (routed as per Figure 3b,c) can only pull, but not push [20]. Thus, the configuration of the mechanism cannot be determined with motor position only, as its torsional mobility is underactuated and depends on the passive spring-like elements in the Appl. Sci. 2021, 11, 2607 5 of 13 system, that is, the torso's backbone. Thus, kinematics must be integrated with a stiffness model of the backbone for complete characterization.
As previously mentioned, a piecewise constant curvature kinematic (PCCK) model is used to describe the main bending of the backbone. PCCK models have been conceived as a convenient tool to analyse and control continuum robots, as explained in [23], even though they are recently being replaced by more complex models with variable curvature [24,25] to accurately describe the hyper-redundant architecture of those systems, with complex motion coupling between independently bending sections. However, as the proposed torso mechanism can be represented by a single bending section, a constant curvature model can appropriately model the compliant backbone with limited approximation errors. To describe the pose of the lower and upper torso, the reference frames {S 0 } and {S} can be defined at the base and at the end of the backbone, respectively. By assuming constant curvature along the backbone, the translation along its central curve t ∈ R 3 can be expressed by an arc of a circle as: while the rotation from {S 0 } to {S} can be written as a rotation matrix R ∈ SO(3) that is: where R z (ϕ) represents a rotation of ϕ around the z-axis, R y (θ) represents a rotation of θ around the y-axis, and R z (∆ϕ − ϕ) represents a rotation of ∆ϕ − ϕ around the z-axis. These equations expand the conventional PCCK that is described in [23] to include both the elongation/compression and the axial torsion that are characteristic of the mechanism under examination. The forward kinematics of the torso mechanism can be thus defined with the homogeneous transformation T ∈ SE(3) from {S 0 } to {S} as: Equation (3) represents only the first part of the kinematics of the structure, as it relates the pose of the upper torso (with respect to the lower torso) only to its configuration, not to the length of the actuating tendons. To compute tendon length l = (l 1 ; l 2 ; l 3 ; l 4 ) T , the four tendons can be modelled as circle arcs that bend in parallel to the backbone, with the same bending angle and direction. Thus, their lengths can be evaluated as: By using Equations (3) and (4), the relationship between actuation vector l and upper platform pose T can be obtained, defining the kinematic input-output function of the robotic system. However, as previously mentioned, the stiffness of the backbone must be considered for full characterization. In order to do so, a linear elastic behaviour of the backbone is assumed, with a torsional stiffness k t , a compression module of k c , and a bending stiffness k b that can be evaluated from the material properties and are related to motion parameters as: where M z is the moment around the z-axis of the backbone, representing axial torsion; F z is the force along the z-axis of the backbone, representing compression; and M θ = M y sin ϕ − M x cos ϕ is the bending moment acting on the backbone, which can be written as a combi-nation of the moments around the xand y-axes of the backbone, M x and M y . Given the tensions in the tendons F 1 , F 2 , F 3 , and F 4 , these values can be computed as: Thus, as expressed in Equation (6) and with reference to Figure 4, the four actuation cables can actively control the two DoFs related to bending, whereas the compression/elongation and torsion of the backbone are coupled and cannot be actuated independently.
Thus, as expressed in Equation (6) and with reference to Figure 4, the four actuation cables can actively control the two DoFs related to bending, whereas the compression/elongation and torsion of the backbone are coupled and cannot be actuated independently.
If a general external wrench ; ; ; ; ; is applied to the upper torso, the elongation and torsion of the backbone can be estimated from: and decoupled through a pose measurement with an onboard motion sensor (for example, with the Inertial Measurement Unit (IMU) installed on the LARMbot's upper torso).
In summary, the model in this section can be used to accurately move the CAUTO torso by integrating the feedback from the motors' sensors and the IMUs on the upper torso and the head of the humanoid robot to enable closed-loop control.

Results
The kinematic model defined in the previous section is here used to evaluate the workspace of the torso mechanism and its characteristics. Furthermore, a simple mapping of the kinematic variables to an input joystick device is discussed for smooth motion planning with experimental validation of a prototype moving in the torso's workspace that is evaluated with the proposed model.

Workspace of the Proposed Torso Mechanism
Whereas the original motion planning was based on a regression between calibration points, the new analytical kinematic model enables a full characterization of the reachable workspace of the robot by defining all the reachable points of the mechanism. By using the forward kinematics in Equation (3), the reachable workspace of the mechanism was evaluated, given the size and motion parameters of the torso prototype in Figure 2, which are reported in Table 2. Once defined the motion limits on angle and direction of bending, as per Table 2, and by assuming a negligible compression of the backbone, the minimum If a general external wrench P x ; P y ; P z ; W x ; W y ; W z T is applied to the upper torso, the elongation and torsion of the backbone can be estimated from: and decoupled through a pose measurement with an onboard motion sensor (for example, with the Inertial Measurement Unit (IMU) installed on the LARMbot's upper torso).
In summary, the model in this section can be used to accurately move the CAUTO torso by integrating the feedback from the motors' sensors and the IMUs on the upper torso and the head of the humanoid robot to enable closed-loop control.

Results
The kinematic model defined in the previous section is here used to evaluate the workspace of the torso mechanism and its characteristics. Furthermore, a simple mapping of the kinematic variables to an input joystick device is discussed for smooth motion planning with experimental validation of a prototype moving in the torso's workspace that is evaluated with the proposed model.

Workspace of the Proposed Torso Mechanism
Whereas the original motion planning was based on a regression between calibration points, the new analytical kinematic model enables a full characterization of the reachable workspace of the robot by defining all the reachable points of the mechanism. By using the forward kinematics in Equation (3), the reachable workspace of the mechanism was evaluated, given the size and motion parameters of the torso prototype in Figure 2, which are reported in Table 2. Once defined the motion limits on angle and direction of bending, as per Table 2, and by assuming a negligible compression of the backbone, the minimum and maximum tendon lengths were computed through Equation (4). The reachable workspace was defined as the geometrical locus of all the points that can be reached by the upper endpoint of the backbone's centreline, and it was computed in MATLAB R2021a as plotted in Figure 5. As expected, for a negligible compression, the operational workspace shape resembles a convex surface. Table 2. Size and motion parameters of the torso mechanism prototype in Figure 2.

Backbone Length
Bending and maximum tendon lengths were computed through Equation (4). The reachable work space was defined as the geometrical locus of all the points that can be reached by th upper endpoint of the backbone's centreline, and it was computed in MATLAB R2021a a plotted in Figure 5. As expected, for a negligible compression, the operational workspac shape resembles a convex surface.

Experimental Validation
To validate the proposed kinematic model, the workspace result was compared t the reachable workspace of the prototype as per Figure 6, which was evaluated in th configuration space of the robot by measuring the orientation of the upper torso. The ex perimental setup is illustrated in Figure 7 with its main components, which include th torso prototype with an embedded IMU on the upper torso on the last Functional Spina Unit (FSU) to extract the orientation data, an Arduino microcontroller with the kinemati model, a power supply, and a joystick (spring-loaded to the centre) to teleoperate the sys tem. All the motors are embedded in the waist of the robot not to hinder spine motion While a wired control was used in this experimental validation, wireless communicatio can enable autonomous navigation with the humanoid robot. The motion of the torso wa mapped to the joystick motion according to the proposed kinematic model, with the d rection of the joystick controlling the direction of bending and the position of the joy stick linearly mapped to the bending angle . The extreme positions of this mapping ar illustrated in Appendix A with the corresponding upper torso pose.

Experimental Validation
To validate the proposed kinematic model, the workspace result was compared to the reachable workspace of the prototype as per Figure 6, which was evaluated in the configuration space of the robot by measuring the orientation of the upper torso. The experimental setup is illustrated in Figure 7 with its main components, which include the torso prototype with an embedded IMU on the upper torso on the last Functional Spinal Unit (FSU) to extract the orientation data, an Arduino microcontroller with the kinematic model, a power supply, and a joystick (spring-loaded to the centre) to teleoperate the system. All the motors are embedded in the waist of the robot not to hinder spine motion. While a wired control was used in this experimental validation, wireless communication can enable autonomous navigation with the humanoid robot. The motion of the torso was mapped to the joystick motion according to the proposed kinematic model, with the direction of the joystick controlling the direction of bending ϕ and the position of the joystick linearly mapped to the bending angle θ. The extreme positions of this mapping are illustrated in Figure 6 with the corresponding upper torso pose. and move around the limits on its workspace, as illustrated in Figure 10, which shows the acquired IMU data points during a manually operated motion around the reachable workspace that was computed with the new analytical model. The experiments also show a negligible backbone elongation and torsion in absence of any external wrench, as motion is here obtained by tendon action only. The proposed model achieves a smooth motion, with acceleration values always below 36 rad/s 2 , as reported in Figure 11. The power consumption is also fairly low, with a maximum value below 15 W, as reported in Figure 12. The experimental results are summarised in Table 3.      In the reported experiment, the robot was driven from a central position (straight backbone) to a point on the outer border of its workspace and then was moved around the entire border once before going back to the central position (see Figure 6). This motion was repeated four times, and the motion data of the upper torso were reported in Figures 8 and 9 as acquired by the IMU. Figure 8 illustrates the acquired motion as bending around the xand y-axes of the torso mechanism, whereas Figure 9 reports the overall bending angle and axial torsion of the torso. The proposed torso mechanism can reach and move around the limits on its workspace, as illustrated in Figure 10, which shows the acquired IMU data points during a manually operated motion around the reachable workspace that was computed with the new analytical model. The experiments also show a negligible backbone elongation and torsion in absence of any external wrench, as motion is here obtained by tendon action only. The proposed model achieves a smooth motion, with acceleration values always below 36 rad/s 2 , as reported in Figure 11. The power consumption is also fairly low, with a maximum value below 15 W, as reported in Figure 12. The experimental results are summarised in Table 3.

Discussion
In this paper, a kinematic model was proposed for a four-DoF tendon-driven compliant torso mechanism for humanoid robots. The proposed model is presented with a closedform analytical formulation and it is used to compute the workspace of the mechanism under examination. The results are validated on a prototype, which is operated with a joystick on which the proposed model is mapped. The main findings of this paper can be summarised as: • Mobility analysis: The mobility of a four-DoF tendon-driven compliant torso mechanism for humanoid robots is analysed to identify its main modes of motion. The degrees of mobility of the robot are further classified as active degrees of freedom, which can be controlled by the action of one or more actuators, and passive degrees of freedom, which depend on the intrinsic stiffness of the system only. • Kinematic modelling: The proposed constant curvature kinematic model, which is usually used for continuum robots, is used to describe the main bending mode of motion (θ, ϕ) of the spine of the torso. The conventional constant curvature kinematic model is characterised for the analysed system, and an expanded model is here proposed to include backbone elongation ∆ and torsion ∆ϕ. • Stiffness modelling: A linearised stiffness model is introduced to create an efficient framework with lumped parameters to relate the deformation of the backbone to the wrench acting on the upper torso of the mechanism. This model also outlines how the axial torsion of the backbone cannot be controlled by the actuation tendons, as it can only be caused by external wrenches. • Workspace analysis: The behaviour of the torso mechanism is characterised by evaluating its motion limit as a reachable workspace, bending and direction angles, and tendon displacement. • Joystick mapping: A joystick mapping of the main motion parameters of the proposed kinematic model is proposed, with direction and angle of bending linearly mapped to joystick orientation and magnitude respectively.
• Experimental validation: The workspace computed with the proposed model is confirmed by experiments with the prototype, whose motion is acquired by an onboard inertial measurement unit.
The main advantages of the proposed model, which replaces a previous approximated model calibrated on experimental data, can be identified as higher accuracy and efficiency and a quick response thanks to the closed-form analytical formulation. Furthermore, this model can be also used to evaluate the operating performance of the robot over the reachable workspace and to ensure that the tendon-driven architecture operates within its wrench-closure workspace.
Future works will focus on refining the current model by introducing a dynamic and stiffness model of the system, which will be validated with the addition of load cells on the tendons; by defining the performance of the torso through numerical indices that can be used to optimise motion planning; by analysing the compliant behaviour of the backbone with Finite Element analysis simulations in order to evaluate the error introduced by the lumped parameter model that is proposed in this manuscript; and finally by integrating this model with the kinematics of the whole humanoid, in order to implement a dynamic control of the system.