Modal-based Kinematics and Contact Detection of Soft Robots
Yue Chen Long Wang Kevin Galloway Isuru Godage Nabil Simaan Eric Barth




















































































































































































































































































































































op-tical net-works semi-conduc-tor














  




















 
 











plain
thmTheorem
definition
defn[thm]Definition 
exmp[thm]Example 




packed_enum_i

		
		
		
	
 












@printindent@tempcnta
@printindent
    @nested>0
        @text@x@notext
            
            -3pt
        @printindent@tempcnta=1
            [ALG@ind@@printindent@tempcnta]
                @printindent@tempcnta 1
            @printindent@tempcnta<@nested+1


@doentity@tlm@printindentfailed to patch






  








 



  
  
  



#1#1 



































































Modal-based Kinematics and Contact Detection of Soft Robots

    Yue Chen, Long Wang, Kevin Galloway, Isuru Godage,
	Nabil Simaan, and Eric Barth
Y. Chen is with the Department of Mechanical Engineering, University of Arkansas, Fayetteville, AR,72701 USA email: yc039@uark.edu 
	
	L. Wang is with the Department of Mechanical Engineering, Columbia University, NY, 10025 USA email: lw2424@columbia.edu 
	
	I. Godage is with the School of Computing, College of Computing and Digital Media, Chicago, 60604 USA email: igodage@depaul.edu
	
	K. Galloway, N. Simaan, and E. Barth are with the Department of Mechanical Engineering, Vanderbilt University, Nashville,TN, 37235 USA e-mail: kevin.c.galloway, nabil.simaan, eric.j.barth@vanderbilt.edu

    December 30, 2023
======================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================

































 
Soft robots offer an alternative approach to manipulate inside the constrained space while maintaining the safe interaction with the external environment. Due to its adaptable compliance characteristic, external contact force can easily deform the robot shapes and lead to undesired robot kinematic and dynamic properties. Accurate contact detection and contact position estimation are of critical importance for soft robot modeling, control, trajectory planning, and eventually affect the success of task completion. In this paper, we focus on the study of 1-DoF soft pneumatic bellow bending actuator, which is one of the fundamental components to construct complex, multi-DoF soft robots. This 1-DoF soft robot is modeled through the integral representation of the spacial curve. The direct and instantaneous kinematics are calculated explicitly through a modal method. The fixed centrode deviation (FCD) method is used to to detect the external contact and estimate contact location. Simulation results indicate that the contact location can be accurately estimated by solving a nonlinear least square optimization problem. 













Pneumatic soft robot, soft robot modeling, contact detection, contact estimation





§ INTRODUCTION


Bio-inspired soft robots are robotic systems made of materials that have the similar moduli order of natural organisms (10^4 - 10^9 Pa) <cit.>. Different from the conventional rigid-link robotic systems with impedance control <cit.> or stiffness modulation <cit.>, compliance is an intrinsic property of soft robots and is achieved through the material mechanical property (for example, the softness of silicone) or morphological control <cit.>. Soft robots enable safe interaction with external objects and exhibit capability to manipulate inside confined environments or achieve complicated tasks. These robots have been used to grasp objects <cit.>, to adapt to environments <cit.>, to estimate external shapes <cit.>, to locomote on rough terrain <cit.>, to regain motor skills in rehabilitation applications <cit.>, to support heart function <cit.> and many more <cit.>.

Soft robots are usually actuated in two forms, namely the variable length tendons and pneumatic/fluidic actuation <cit.>. Pneumatically actuated soft robots offer certain advantages due to the benefit of high power-to-weight ratio and easy implementation. Pneumatic muscle actuators (PMA) or McKibben actuator, a simple one degree of freedom (DoF) pneumatic soft actuator fabricated by a rubber tube and fiber sleeves, was proposed for artificial limb research since 1950's <cit.> and commercialized in the 1980s by Bridgestone Company. The history of  pneumatically actuated soft robotic system dates back to 1992 when Suzumori et al. developed a 3-DoF (pitch, yaw, and extension) microactuator <cit.>. Thanks to the widespread availability of 3D printing technique and affordable fabrication materials, the design and fabrication of pneumatically actuated soft robotics has been continuously growing in the past 10 years. For instance, a pneumatically-actuated soft bending actuator can be fabricated by (1) fabricating the outer molds of the soft actuator, (2) pouring the silicone to the molds, (3) leaving the silicone to cure, and (4) demolding the actuator <cit.>.

Forward and inverse kinematic modeling of pneumatic soft robot are required to understand the robot inherent characteristic and enable accurate closed-loop control. In practice, each segment of a soft robot can be regarded as a continuum section when it is powered. Several approaches have been reported to study soft or continuum robot modeling. Piecewise constant-curvature assumption has been widely used in different soft/continuum robot forward kinematics modelings, such as the D-H parameter approach <cit.>, Frenet-Serret frames approach <cit.>, exponential coordinates approach <cit.> (see the detailed continuum robot kinematic modeling in <cit.>). This assumption provides a simple method to obtain the mapping from robot configuration space to task space. However, the piecewise constant curve assumption is not valid for all the conditions (see Figure 4 of <cit.> or Figure <ref> in this paper), especially when the shape of a soft or continuum robot is significantly changed due to the large internal joint space pressure input, requiring more accurate modeling and control. A more general approach is a modal representation proposed by Chirikjian et al. in 1994 <cit.>, which described the kinematics of a hyper-redundant snake robot with both constant and variable curvature. Wang and Simaan <cit.> adapted this modal representation for multi-backbone continuum robots and presented kinematic error propagation and geometric calibration to address equilibrium shape deviation. Similar to the rigid serial robot, the inverse kinematics of soft continuum robot is more challenging compared to its forward kinematics. Neppalli et al. developed a  closed-form geometric approach by modeling each segment of a continuum robot as a spherical joint and rigid link and using the conventional analytical method to solve the inverse kinematics <cit.>. The Jacobian-based approach is able to search the arc parameters to achieve the desired configuration <cit.>. Garbin et al. recently proposed a design that mechanically couples a pneumatic parallel bellows actuator (PBA) with a multi-backbone continuum user interface (CUI), and presented a unified kinematics for both the PBA (soft robot) and the CUI (continuum) <cit.>. 

With the aforementioned kinematic modeling approaches, the soft robots could be accurately controlled in the free space environment. However, due to the compliance of the soft robot, an external wrench disturbance (e.g. force or torque) in a realistic environment could potentially lead to significant robot configuration variation, resulting in inaccurate control. To estimate the external wrench applied to the soft robot body, Ozel et al. proposed a soft robot curvature sensing technique by integrating a resistive flex sensor or a magnetic curvature sensor <cit.>. Kramer et al. proposed a polydimethylsiloxane (PDMS) based curvature sensing technique in 2011 <cit.>. However, the soft robot kinematic model variation caused by the external contact remains unsolved.





Inspired by the work done by Bajo <cit.>, this paper aims (i) to develop a new kinematic modeling approach for soft robot with generalized configurations and (ii) to enable accurate contact detection during motion with feasible sensing feedback. We will systematically study the kinematic modeling of soft robot with contact, contact detection and contact localization. The contact force magnitude estimation is out of the scope of current work. We use an 1-DoF pneumatic bending actuator <cit.> to experimentally validate the modeling method. 
This paper is a step toward our overarching goal of robust soft robot control method for actual applications when the soft robot is in contact with the external objects. 
The rest of the paper is arranged as follows. Section <ref> details the research problems and the basic modeling assumptions. Section <ref> describes the theoretical kinematic modeling and contact detection algorithm. Section <ref> presents the simulation results followed by Conclusions in section <ref>.







§ PROBLEM STATEMENT


Figure <ref> shows the soft robot prototype, which consists of a pneumatic bellow soft actuator and a manual pneumatic pump. The actuator fabrication process with fiber reinforcement technique was detailed in <cit.>. The cross-section of pneumatic bellow actuator is semicircle shape, which has shown to have the smallest bending resistance compared to the rectangular shape and circle shape. A strain-limiting layer was added to its radial base to achieve the uni-directional bending motion and to prevent the axial extending and contracting motion. The circumferential reinforcement wires prevent the pneumatic bellow from significant radial expansion at high joint space pressure input. The maximum bending angle is occurred when the input pressure is 25 psi. 

In this paper, we aim to address the following three problems: 

	
   Problem 1: Derive the generalized kinematic model of the 1-DoF pneumatic soft bellow actuator. Find the direct kinematics and instantaneous kinematics that maps from robot actuation space to task space.
	
   Problem 2: Given the soft bellow end effector position and velocity, determine whether the external contact is applied to the bellow or not.
	
   Problem 3: Given the soft bellow end effector position and velocity, find the contact location along the backbone of the bellow if the external contact is applied to the robot. 	


When discussing these problems, we use the following modeling assumptions:

	
  * The pneumatic bellow bends in the plane. 
	
  * The curve of pneumatic bellow and its first-order derivative are continuous.
	
  * The pneumatic bellow end effector position and orientation can be obtained from an extrinsic sensor (e.g. electromagnetic tracker).
	
  * The portion of a soft bellow from base to contact position maintains its shape after contact and the unconstrained portion continues to bend as if it was a shorter bellow. 







§ MATERIALS AND METHODS




 §.§ Forward Kinematics: Actuator Space to Task Space


The kinematics of continuum robots can be described using two concatenated mappings according to <cit.>. The first one is a mapping from actuator space (input air pressure q in this paper) to configuration space (tangential angle θ_s along the bending curve at the arc length of s and the given input pressure q). The second one is from the configuration space (tangential angle θ_s)  to task space (soft actuator tip position and orientation). With the piecewise constant-curvature assumption <cit.>, the robot end effector position in the inertial frame can be written as 𝐩=[r(1-cosα),   0,  rsinα]^T. The end effector orientation can be achieved by rotating about the y-axis for α degree (see Figure <ref>). Therefore, the homogeneous transformation from robot based to end effector can be written in equation <ref> 

    𝐓    =[[ R_y(α)      𝐩;      0      1 ]] =[[     cosκs         0     sinκs 1-cosκs/κ;         0         1         0         0;    -sinκs         0     cosκs   sinκs/κ;         0         0         0         1 ]]

where s is the arc length and κ = 1/r is the curvature. However, this forward kinematics expression is problematic when the curvature is zero. As can be seen from figure <ref>, zero curvature indicates that the continuum robot is a straight line, while the last column of homogeneous transformation matrix is undefined. 



To address the above expression singularity and model the soft robot when the piecewise constant-curvature assumption is insufficiently accurate, we use the modal approach to describe the pneumatic bending characteristics from the actuator space to configuration space. Once the soft pneumatic bellow is powered and at the static position, the geometrically exact configuration only varies with respect to the input pressure and is determined by the minimal energy solution. Experimental calibration is performed with the hardware platform in figure <ref> to obtain a family of shapes under the specific pneumatic pressure input. The mapping from actuator space to configuration space can be approximated in the following equation <cit.>: 

    θ(s,q)=ψ(s)^TAη(q)

where ψ(s) and η(q) are described in the following modal representations

    ψ(s)=[1, s, s^2, …, s^v-1]^T


    η(q)=[1, q, q^2,…, q^w-1]^T

The bending actuator backbone can be decomposed into g discrete points along the strain limiting layer. Each point has a specific corresponding tangential angle at z different input pressure. Then equation <ref> can be expressed in the following matrix form:

    Φ=Ω_g×vA_v×wΓ_w×z

where Φ_i,j=θ(s_i,q_j), i is the number of discrete points along the bending actuator backbone and j is the number of input pressure.  Ω and Γ are written as follows

    Ω    =[[           1         s=0           …     s=0^v-1;           1       s=s_1           …   s=s_1^v-1;           ⋮           ⋮           ⋱           ⋮;           1     s=s_max           … s=s_max^v-1 ]]_g×v



    Γ    =[[           1           1           …           1;       q=q_1       q=q_2           …     q=q_max;           ⋮           ⋮           ⋱           ⋮;   q=q_1^w-1   q=q_2^w-1           … q=q_max^w-1 ]]_w×z












Equation <ref> can be reshaped into the following form:
 
    [Φ^T⊗Ω] 𝐀=Φ

where ⊗ is the Kronecker's product, 𝐀 and Φ are the vectorized form of A and ϕ respectively, which can be written as:
 
    𝐀 = [a_11… a_v1, a_12… a_v2,… , a_1w… a_vw]^T


 
    Φ = [θ_11…θ_g1, θ_12…θ_g2,… , θ_1z…θ_gz]^T


Solving matrix 𝐀 could get the mapping from the soft actuator joint space to the configuration space. 

The mapping from  configuration space to task space can be explained by using the definition of curvature (see figure <ref>), as described in the following equation: 

    d𝐱_s/ds=[cosθ_s  0   sinθ_s ]^T

where 𝐱_s is the point position expressed in the inertial frame at the arc length of s and θ_s is the corresponding tangential angle s (figure <ref>). 
The forward kinematics that maps from actuator space and task space is obtained by substituting equation <ref> into equation <ref> and integrate the tangent vector along the arc length.  




 §.§ Instantaneous Kinematics 


This section describes the instantaneous kinematics modeling that maps the actuator speed q̇ to the end effector twist  ẋ=[v^T,   ω^T]^T. The end effector twist consists of the linear velocity v and angular velocity ω. However, since the actuator bends in the x-z plane, the linear velocity in y direction and angular velocity about x and z axis are zero. We seek to find the Jacobian J_xp that satisfies the following equation:

    ẋ = J_xqq̇


The Jacobian the relates the joint space motion rates q̇ with the end effector twist can be derived by using the differential chain rule: 




















    J_𝐱q    = ∂𝐱/∂q
       = ∂/∂q∫_0^L[cosθ_s  0  sinθ_s ]ds 
        =  ∫_0^L[∂/∂qcosθ_s  0  ∂/∂qsinθ_s ]ds

where ∂/∂qcos(θ_s) and ∂/∂qsin(θ_s) can be written as follows: 


    ∂/∂qcos(θ_s)     = -sin(θ_s)∂(ψ(s)^TAη(q))/∂q
       = -sin(θ_s)ψ(s)^TA ∂(η(q))/∂q









    ∂/∂qsin(θ_s)     = cos(θ_s)∂(ψ(s)^TAη(q))/∂q
       = cos(θ_s)ψ(s)^TA ∂(η(q))/∂q









Combining equation <ref>, <ref> and <ref>, we can get the Jacobian that relates the actuator space speed to the robot end effector twist (linear and angular velocities). Given the specific arc length L, the end effector twist is the function of input pressure q and pressure change rate q̇. The Jacobian was also validated numerically by using the resolved rates method <cit.>, as can be seen in the following equation: 



    q_i = q_i-1+ J^-1α(𝐱_des - 𝐱_i)


where α is the positive scaling factor.




 §.§ Contact Detection

This section describes the contact detection algorithms as initially discussed in <cit.>. The joint force deviation (JFD) method calculates the external wrench value based on the deviation of generalized joint space forces with respect to the nominal model. It can be described in the following equation: 

    J_qθ^T τ =∇(E)-J_xθ^T W_e

where ∇(E) is the gradient of elastic energy with respect to the configuration perturbation, τ and W_e are the joint force and external wrench respectively. When there is no external wrench applied on the soft robot, then J_qθ^T τ =∇(E). Once the wrench applied on the robot (W_e 0), which leads to the variation of the joint force if the pre-determined detection threshold is ξ=0. Based on the deviation value, we can obtain the external wrench according to the equation below: 

    W_e = -(J_xθ^T)^-1J_qθ^TΔτ

where Δτ is the deviation of joint force. In the real applications, the contact detection threshold ξ is typically not equal to 0, which is caused by the friction force in the pull-wire continuum robots and the compressibility characteristic of pneumatic supply
in the current robot. To implement this algorithm, one needs to have a accurate joint space pressure sensor to get the generalized joint force feedback. However, the major limitation of this algorithm is that it does not provide the contact location <cit.>.  

The fixed centrode deviation (FCD) method calculates the deviation of theoretical and actual loci of the fixed centrode of end effector to detect the contact position. The loci of theoretical fixed centrode (or instantaneous screw axis, ISA) can be expressed in the following equation <cit.>:

    c_m=Ω_m(𝐏̇_m - Ω_m 𝐏_m)/ω_m^Tω_m
 
where 𝐏̇_m and ω_m are the end effector linear velocity and angular velocity respectively, which is calculated from the instantaneous kinematics model described in section <ref>. Ω_m is the skew-symmetric angular velocity matrix defined as Ω_m=[ω_m]^∧ <cit.>. Based on the modeling assumption (3) in section <ref>, the actual end effector position and orientation can be captured through a tracking system (e.g. EM tracker or motion camera). The loci of actual fixed centrode is expressed as following:

    c_s=Ω_s(𝐏̇_s - Ω_s 𝐏_s)/ω_s^Tω_s
 
where the subscript s in equation <ref> indicates the sensor data. Once the deviation of fixed centrode occurs, there exists an external wrench along the pneumatic bellow soft robot. Similarly, the detectability threshold ξ also exists in the FCD method and it is affected by the tracking system resolution.



 §.§ Contact Location Estimation


This section presents the contact location estimation algorithm by using the kinematics modeling and contact detection method described above. Once the contact occurs at a given location s=s_c, the robot kinematic model can be decomposed into two sections (see figure <ref>. Note that, without future explanation, x-position and z-position are expressed in the unit of pixel, which is 0.2959 mm/pixel). Due to the inherent compliance characteristic of the soft robot, the soft robot base to contact position forms the constrained portion. The contact position to end effector forms the unconstrained portion, which continues to bend with the increase joint space pressure. The unconstrained portion kinematic model is similar to the model described in section <ref> and <ref>, which has a shorter arc length and different base position <cit.> (which is the contact location). The modified kinematic model can be written as:
 
 θ̃(s,q)=
 	θ_c(s,q_c)     0<s≤ s_c 
 	
 	

 	ψ(s-s_c)^TAη(q)     s_c<s≤ L 
 	
 
where q_c is the input pressure when contact occurs, s_c is the contact location, and θ_c(s,q_c), s∈ [s_c, L] is the tangential angles along the arc when contact occurs. 


Having this relationship, the pneumatic bellow actuator instantaneous kinematics can be obtained readily by changing the actual arc length.

    ẋ̃̇ = J̃_̃x̃q̃q̇
  
where J̃_̃x̃q̃ is the Jacobian of the unconstrained portion by substituting the arc length s = L-s_c into equation <ref>, <ref> and <ref>. Then the loci of the fixed center can be derived by combining equations <ref>, <ref>, <ref>, <ref>,  <ref>, and  <ref>. 

The contact location is estimated by solving a least square optimization problem that minimizes the Euclidian distance between the actual loci of fixed centrode obtained from sensor data and theoretical loci of the fixed centrode obtained from modeling results. The objective function is given by 

    argmin1/2c_err^TW c_err

where c_err = c_s-c_m and W is the weighting matrix. The gradient of fixed centrode c_m in terms of contact location s_c needs to be derived to implement this algorithm. According to equation <ref>, the gradient can be written in the following form:

    ∂c_m/∂ s_c=∂c_m/∂ω_m∂ω_m/∂ s_c+ ∂c_m/∂𝐏̇_m∂𝐏̇_m/∂ s_c+∂c_m/∂𝐏_m∂𝐏_m/∂ s_c+∂c_m/∂ω_m∂ω_m/∂ s_c


With the gradient calculated above, the least square optimization problem will be solved iteratively according to <cit.>.










§ RESULTS


This section describes the results of pneumatic bellow calibration performance, forward/instantaneous kinematics, motion simulation after contact, and contact position estimation. 



 §.§ Pneumatic Bellow Calibration


This section presents the pneumatic bellow calibration results. The experimental setup includes a soft bending bellow, manual pneumatic pump, a high resolution camera (C920 Webcam, Logitech, Swiss) and custom designed graph user interface (GUI). The manual pneumatic pump has the resolution of 1Psi. High resolution input pressure control can be achieved by closing the pressure control loop with a digital pressure sensor. The pneumatic bellow with different air pressure inputs was captured by the camera system and stored for post processing. In this experiment, the input pressure samples 0 Psi, 6 psi, 10 Psi, 15 Psi and 21 Psi respectively  (Figure <ref>).

The strain limiting layer on the pneumatic bellow was manually segmented to represent the pneumatic bellow shape. We annotated ten points along the each backbone and find the corresponding positions and tangential angles. By solving equation <ref>, we can obtain the model-dependent 𝐀 matrix. The segmented curve and calibrated model can be seen in Figure <ref>. The result shows that the calibrated model closely matches the experimental results. The difference between the calibrated model and the actual shape increases with the increase of input pressure. However, the maximum error occurred at 21 Psi input pressure is less than 2.1 mm. Increasing the number of point g could increase the calibration accuracy at the expense of increasing the computation load. 












 §.§ Instantaneous Kinematics Evaluation



This section validates the instantaneous model of the pneumatic bellow actuator as presented in equation (<ref>). Given the actuator initial position and desired position, the resolved rates algorithm is applied to find the corresponding joint space pressure input. Note that the instantaneous kinematics model is evaluated in the scenario of no contact occurs. Figure <ref> shows the iterative process when the pneumatic bellow converges to the desired position. The pneumatic bellow actuator end effector space error is 0.0326 mm (0.0978 pixels) and the joint space error is 0.016 Psi, which validates the accuracy of the proposed instantaneous kinematics model.   



 §.§ Contact Detection and Estimation


This section presents the contact detection and contact location estimation. In the experimental study, the contact occurs at s_c = 100. The input pressure was increased from 5 Psi to 20 Psi with the step size of 0.05 Psi. The loci of fixed centrode throughout the trajectory was calculated simultaneously. We also provided the robot motion without any contact for comparative study. The simulation results are shown in Figure <ref>, which illustrates that the ISA difference between these two scenarios increases steadily due to the growth of input pressure. This is mainly due to the significant end effector position difference as the pressure goes up, see equation (<ref>). Maximum ISA difference is observed when the input pressure is 20 Psi. 


We also simulate the relation between ISA difference vs. the contact location. In the simulation, the contact position was chosen at s_c=50 to 400 from the actuator base with 50 increment per step (Figure <ref>). The performance index is defined as follows:

    norm(c_c-c_f)

where c_c is the fixed centrode when robot is in contact and c_f is the fixed centrode when robot is free of contact. The ISA difference serves the sensitivity performance for the FCD method and it rises as a result of the contact location moves further from the base. Intuitively, contacts can be detected easily when they are closer to end effector due to the large difference in both end effector twist and position when such contacts occur. In contrast, it is difficult to detect contacts when they are close to the base. The ISA difference equals to 0 when s_c=0 (indicates that the contact point is located at the fixed base of the soft bellow actuator), which also represents that the contact does not affect the robot shape and the ISA. 










Next we present the contact position estimation result by using the method discussed in section <ref>. The ISA with a given contact location (s_c = 100) is obtained from the simulation results and used as the ground truth. The estimation process starts with an initial guess (s_c = 200) and then calculates the ISA error as well as the ∂c_m/∂ s_c. Using the nonlinear least square optimization method, the solution iteratively converge to the ground truth. Another initial guess (s_c = 20) was also tested. Both cases studies were able to find the contact position with the acceptable accuracy (figure <ref> and <ref>).











§ CONCLUSIONS


This paper presents the a mathematical framework for kinematic modeling, contact detection, and contact location estimation of a 1-DoF soft bellow actuator. A modal representation approach was used to derive the forward and instantaneous kinematics model.  We presented the simulation study of the pneumatic bellow actuator with contact by using a modified kinematic model. 
The fixed centrode deviation was used as the performance index for contact detection. Contact position was estimated by solving a nonlinear least square optimization problem. Simulation results show that the proposed method could accurately estimate the contact location with sub-pixel error. 

The results in this paper show for the first time how the mapping from joint space and task space can be generated through the modal approach and how the contact detection could be estimated through a least square optimization manner. This will inspire future development of complex soft robot modeling and contact control to accomplish the desired tasks. 



IEEEtran






















