Nakagawa and Mochiyama’s shape estimation algorithm can theoretically be used for CTRs, since the elastic tubes are also modeled as Kirchhoff rods in kinematic modeling. However, pre-curvature is present even without applied forces and torques. We first show how the pre-curvature of tubes can be considered.

In order to reconstruct the shape of the CTR from measured wrenches at the base with Nakagawa and Mochiyama’s algorithm, we consider the undeformed robot’s backbone as a single discrete kinematic chain, which comprises a series of links and joints. Starting from the base that is fixed to a F/T-sensor, Mochiyama et al. number the joints from 0 to n − 1. In contrast to that, we assume the tube consists of m segments, such that we number the joints for segment j from 0 to n _j − 1. On each joint, i.e. on joint k, a local coordinate system R _k ∈ SO(3) is attached to joint k with its z-axis coinciding with the central axis of the previous link. An exception is the base joint, whose coordinate system is the same as the origin. Virtual torsion springs are assigned to each axis of the joint coordinate system. If external loads are present, the torques provided by those virtual torsion springs can be seen as a discrete frame-invariant representation of the internal moments along the rod and can be calculated with the discrete Euler-equation (Yamada et al., 2007; Takano et al., 2017) to further determine the rotational movement of each joint parametrised by Tait-Bryan angles: θ k = θ k , z θ k , y θ k , x T ∈ R 3 . (1)

We represent the tubes in our shape estimation algorithm as a combination of segments with constant curvature, where each segment j lies on a different equilibrium plane. This is a common approach for modeling the CTR’s kinematics (Robert J. Webster and Jones, 2010). Note that straight segments of the tubes are described by a curvature of 0 mm⁻¹.

We aim to find the initial configuration of the kinematic chain, described by joint angles θ k * and local rotation axes a k * at each joint, by approximating the constant curvatures of each segment j of a tube i as the angular rate of change of the local Frenet frame (Robert J. Webster and Jones (2010)) with respect to the arc-length s. Throughout this work we consider, without loss of generality, that pre-curvature is applied locally around the y-axis of a segment: u * ( s ) = ( R T ( s ) R ̇ ( s ) ) ⊕ = 0 κ y 0 T , (2) where ⊕ is the conversion operator for an element of so (3) to its corresponding element in R 3 . Instead of discretizing the curvature using osculating circles for each segment (Bobenko et al., 2008), we discretize the tubes into a link chain with an equal link length of l. In our approach, we represent pre-curvature as virtual external loads which deform an originally straight elastic rod to its pre-curved state (cf. Figure 2). We approximate the deformed shape, using an initially straight discrete serial chain, by assuming the energy stored in the virtual torsion springs along the y-axis of the kth local joint coordinate system (Mochiyama, 2016) is equal to the energy stored in a deformed segment section between kl and (k + 1)l. Since we assume a planar pre-curvature, the local curvature indicates that each segment of the robot only bends about the local y-axis, such that the joint angles and rotation axes can be obtained by solving the following equation: 1 2 ∫ k l ( k + 1 ) l u * ( s ) T K u * ( s ) d s = 1 2 ξ j , y θ k * 2 , (3) where K = d i a g E i I i E i I i G i J i is the stiffness matrix constituted of stiffness of the j-th segment, describe the initial joint angles of the kinematic chain and ξ _j,y is the spring constant for each segment section. Note, in the case of constant pre-curvature, the calculation of the forming energy simplifies to: E = 1 2 l u j * T Ku j * . By assuming ξ _j,y = E _i I _i , which is for all joints in the same segment equal based on the constant curvature assumption, and that the curvature is a rotation around the local y-axis e _y about ξ _j l, we can solve towards θ k * by canceling E _i I _i on both sides and yield: θ k * = lu j * . (4)

We now consider the application with multiple tubes. Thus the configuration of the tubes relative to each other must be considered. Note that we make the assumption that the tubes cannot be twisted relative to another. Further, we assume that the stiffness is independent of the configuration. Given the constant curvature assumption, we can apply the CTR-specific arc parameter mapping stated by Robert Webster and Jones (2010). Note, we assume that the local z-axis of each frame along the tube coincides with the backbone of the robot. Each tube i can be rotated around the z-axis of the base by an angle α _i . Furthermore, each tube can be extended and retracted, which is described by the parameter β _i ∈ [−L, 0], where β _i = 0 describes a fully extended tube. From the manufacturing of the tubes, it is known where each tube’s straight and curved part starts and ends, which can be described as a sub-interval of arc length s ∈ [0, 1] parameterizing the tubes’ backbone. Parameterized by s each end of a straight or curved part marks the end of a distinct constant curvature segment on the CTR’s backbone.

For each segment j, we can determine the pre-curvature κ _j considering that moments will be constant along the segment. Given the κ i * which is the pre-curvature of the tube i, we can determine the curvature components: κ x , j = ∑ i E i I i κ i * ⁡ cos α i ∑ i E i I i κ y , j = ∑ i E i I i κ i * ⁡ sin α i ∑ i E i I i , (5) where E _i is the Young’s modulus, I _i the second moments of area about the local x- and y-axis, which we assume to be equal, and α _i the rotation around the z-axis of the tube i. Because all tubes share the same neutral axis and we assume that the material is isotropic, we can sum up their individual stiffness values. Given the projections of the curvatures onto the new plane intersecting the combined tubes, we can calculate the combined curvature: κ j = κ x , j 2 + κ y , j 2 . (6)

The deformed shape of the robot, as well as the position and orientation of the robot tip, can be then calculated with the given base position p ₀ and orientation R ₀ recursively: R k + 1 = R k R o t ( e y , κ j l ) R o t ( θ k ) , p k + 1 = p k + l R k + 1 e z . (7)

We follow the work of Nakagawa and Mochiyama (2018) and also consider the influence of the gravitational forces on the tubes. Therefore, for each segment section of length l _j we calculate the mass M _j with respect to the set of tubes I acting in this section: M j = l j L ∑ i I M i , (8) where we assume that the weight is uniformly distributed over a tube and L is the length of the whole backbone. The calculation for segments is described in pseudo-code in Alg. 1, where c = p k + p k − 1 2 for the compensation of gravitational forces (Nakagawa and Mochiyama, 2018). All input and output model parameters are listed in Alg. 1, too. Note that the code in Alg. 1 is executed for each segment j of the backbone of a CTR. Given m segments and n sections the overall computational complexity increases to O ( m ⋅ n ) where for good accuracy m ≪ n.

Algorithm 1: Shape estimation algorithm for each segement of a CTR with precurved tubes

We used a Kuka LBR iiwa 7 R800 robot (Kuka AG, Germany) for moving the tube with a JR3 F/T-sensor 50M31 A-I25 (JR3, Inc., United States) attached at the flange. Two custom-made 3D-printed clamps fixed the tube to the F/T-sensor and a table (Figure 3A) shows the general setup of the clamps). We printed three 150 mm tubes with a constant curvature of 1/95 mm⁻¹, 1/140 mm⁻¹ and 1/260 mm⁻¹, resembling a bending angles of 90°, 60°, and 30°. Additionally, we printed small belt line hooks at the ends of the tubes to keep them fixed. For the two-tube experiment, we printed one inner tube with a length of 150 mm, and three different outer tubes of length 50 mm, 75 mm, and 100 mm. All tubes have been made of taulman3D 618 Nylon (taulman3D, United States). Although, nylon has a higher degree of plastic deformation it has been shown that CTR made of nylon achieve similar accuracies as robots made of NiTi (Amanov et al., 2015; Morimoto and Okamura, 2016). We chose the inner and outer diameters for the applied tubes such that the plastic deformability is low and the tubes are printable with an Ultimaker3 (Ultimaker, United States). Due to fabrication limitations, it was only possible to produce a rod with a diameter of 2.8 mm. A thin rod was preferred over a thicker one for more elasticity. Further parameters are listed in Table 1, where L _s describes the length of the straight part of the tube and L _c its pre-curved part, respectively. The algorithm was implemented in Python3.8 and run on an Intel Core i7-7,700@4.5 GHz PC. Communication with Kuka Sunrise.Os and the F/T-sensor was handled with Robotics Service Bus (RSB) (Wienke and Wrede, 2011) and ROS Melodic Morenia (Quigley et al., 2009).

For the evaluation of our proposed algorithm on a single pre-curved tube, we followed an experiment protocol similar to Nakagawa and Mochiyama (2018) and applied planar deformation and torsional deformation separately, and in combination on the tube. Figure 3A) shows an undeformed tube, where the world coordinate system is fixed in the clamp at the end of the tube and the tube’s base is mounted in the clamp at the F/T-sensor which is mounted at the robot flange. The z-axis of the world coordinate system is tangent to the tube centerline and the negative x-axis is directed to the center of the osculating circle with radius of r = 1 κ . The position and orientation of the base of the tube with respect to the world coordinate system is defined as follows: p 0 = − 1 − cos ⁡ κ L κ 0 1 − sin ⁡ κ L κ R 0 = − cos ⁡ κ L 0 sin ⁡ κ L 0 1 0 − sin ⁡ κ L 0 − cos ⁡ κ L (9)

This information is used to transform the calculated shape from the sensor coordinate system to the world coordinate system. The pose of the end of all tubes is set as follows and is compared with the calculated pose: p n = 0 0 0 R n = − 1 0 0 0 1 0 0 0 − 1 (10)

Like in the evaluation of the algorithm of Nakagawa the tubes are deformed by bending and torsion. In the following, the different test poses, based on the position and orientation of the base of the tube after deformation p ₀ and R ₀ are presented.

Since fixing the tubes severely restricts the workspace of a CTR and combined tubes cause complex movements when rotated into each other for larger rotations, we chose a different recording scenario for evaluating our algorithm with multiple tubes. Using a motion capture system, the shape of the robot’s tubes is tracked to obtain accurate position measurements. For this purpose, we applied 5 mm wide retro-reflective tape (3M, Germany) to the tubes by attaching 15 markers separated by a distance of 5 mm. These markers were recorded with seven cameras of the motion capture system (Optitrack-Prime-13-System, NaturalPoint, United States) with a mean position error of 0.235 mm.

The printed tubes were rotated using a custom-made drive unit consisting of two stepper motors and a 3D-printed gear box (c.f. Figure 5). Lacking a translation joint in our actuation unit due to the integration of the F/T-sensor, we printed three outer tubes of different lengths as mentioned above to emulate elongation. We assumed that the outer tube can elongate a maximum of 100 mm. As mentioned previously, due to limitations in the additive manufacturing of the small 3D-printed tubes, the inner tube was produced as a nylon rod. An additional ring was printed on its tip to attach two different weights (15 and 25 g) for applying load to the tubes.

We recorded 9,814 data samples without applied loads to an outer tube elongation of 100 mm and a step width of 1° for the tubes’ rotation. Furthermore, we recorded 2,863 data samples with applied loads for the three described outer tube lengths with a step width of 9°. Each single sample recording without, applied loads lasted 0.5 s to reduce the possibility of frictional snapping effects. Each sample recording with applied load to the tubes’ tip lasted 6 s, consisting of 5 s to relax vibrations introduced by the inertia of the applied weight at the end of the tube plus 1 s to measure and average the measured force and torque. The recorded movements were limited to the range [−π/2, π/2] (rad) for the inner and outer tubes.

The restrictions have been introduced because a rotation over π/2 rad did not lead to any significant tip movement due to the applied weight. The resulting torsional forces twisted the tube and snapping effects occurred when overwinding, which represents a special case in the application of CTRs we did not want to investigate. The outer tube performed one step in a positive direction of rotation as soon as the inner tube had reached the angle − π/2 or π/2. After recording, samples with missing positions along the shaft or additional points due to noise were dropped. The recording setup is shown in Figure 5.

Based on the recorded wrenches, we calculated the corresponding virtual positions (positions given by our algorithm derived in Section 2) for each configuration. As discussed above, 15 points were recorded with the tracking system and the same number of virtual points were generated per sample. A comparison of measured and corresponding calculated tip positions with respect to the measured forces is shown in Figure 6. We calculated the maximum absolute error (MAE) between the virtual (model-based calculated) and measured points along the robot shaft and the inner tube’s tip position and calculated the standard deviation (STD) of the error. For the data without loads applied to the robot, we achieved a MAE of 3.63 ± 3.32 mm for all points along the shaft and 1.74 ± 1.98 mm for the tip position deviations. Table 5 shows the results for all six tube-weight combinations. The best tip position MAE of 4.47 ± 2.94 mm and the best MAE of 1.39 ± 1.68 mm accounting for all positions along the backbone was achieved for an outer tube of length 100 mm and 15 g tip load.

In contrast to the results for individual tubes in Section 3.2.5, the results for multiple concentric tubes show larger variances in error. Specifically, for the deviation in z-direction our model is overestimating the deflection. This is particularly evident in the results with an outer tube of only 50 mm, where the deviations are in the centimeter range, caused by larger bending due to the weight.

As Takano et al. (2017) state in their work, the approximation of the bending by the discrete Euler-equation produces larger deviations to the actual curvature, if the tubes experience a strong bending. In addition to that, larger deflection as given in this evaluation lead to relative movement between the tubes, which we do not consider in our model. Note that such large deflections are not commonly assumed for tubes with such stiffness. Still, a solution should be found in further investigations, however, this was not the main focus of this work and the tests with larger deflection show the limitation to our approach. Further, smaller factors for the deviations occur partially due to the evaluation setting, in which the greater weight of the actuation unit generates an additional force, which is substracted out, but still affects the sensor readings. Furthermore, the JR3 FT-sensor as displayed in Figure 5 has a slight coupling between the force and torque axes, which increases the measurement error. Another minor factor is the inaccuracy in the determination of the material properties. We assume a symmetrical cross-section, which, however, is an idealization and not possible to be achieved perfectly with the applied printing process. The disadvantage of dependence against sensor data quality and well chosen model parameters is also mentioned by Takano et al. (2017).

Since we calculate the positions along the backbone by m ⋅ n discrete transformations the errors along the tubes add up. It can be seen that the deviation towards the tip of the tubes becomes more and more pronounced, which can be explained by the structure of the discretized model, which accumulates errors through the iterative calculation from the basis to the tip. This is, however, inevitable, as we rely on a measurement at the basis.

Continuous models achieve better results when large deflections are present and tip position and orientation is given. However this information is not always available and the proposed approach can estimate the shape solely based on the wrench information at the basis. Furthermore, the results have a favorable trade-off between good accuracy at highly efficient computation as the algorithm always produces the same result in the same duration. It makes the approach applicable for hard real-time applications, where cycle deadlines always need to be met, and which are necessary for ensuring the safety of medical applications.

To evaluate our assumption about real-time application, we evaluated the computation time of the algorithm, which depends on the number of links, i.e. the number of computational steps required. For n = 50, an average computation time of less than 3 ms was measured, which enables an estimation frequency of ca. 333 Hz. This is sufficiently fast for real-time shape estimation. However, if the sensor is noisy, a filter is needed to improve the shape estimation results. In this work, a fifth-order Butterworth low-pass filter was used to suppress noise. This delays the calculation, but still a shape estimation with 20 Hz can be achieved which can be hardware optimized in potential real applications. Given that the algorithm always produces a response after n ⋅ m calculation steps, we concluded that hard real-time deadlines for control can be met.