Zhongkai Zhang
email: zhongkai.zhang@inria.fr

Antoine Petit
email: antoine.a.petit@inria.fr

Jeremie Dequidt
email: jeremie.dequidt@inria.fr

Christian Duriez
email: christian.duriez@inria.fr

Calibration and External Force Sensing for Soft Robots using an RGB-D Camera

Benefiting from the deformability of soft robots, calibration and force sensing for soft robots are possible using an external vision-based system, instead of embedded mechatronic force sensors. In this paper, we first propose a calibration method to calibrate both the sensor-robot coordinate system and the actuator inputs. This task is addressed through a sequential optimization problem for both variables. We also introduce an external force sensing system based on a realtime Finite Element (FE) model with the assumption of static configurations, and which consists of two steps: force location detection and force intensity computation. The algorithm that estimates force location relies on the segmentation of the point cloud acquired by an RGB-D camera. Then, the force intensities can be computed by solving an inverse quasi-static problem based on matching the FE model with the point cloud of the soft robot. As for validation, the proposed strategies for calibration and force sensing have been tested using a parallel soft robot driven by four cables.

I. INTRODUCTION

During the last decade, many works have been done for the design, modeling and control of soft robots [START_REF] Rus | Design, fabrication and control of soft robots[END_REF]. However, calibration and force sensing for soft robots are still open and challenging research areas, especially for robots with complex structures. Although deformations makes modelling difficult for soft robots, their observation can provide information for calibration and force sensing, which this work attempts to demonstrate.

Robotic calibration can be a key requirement to increase the accuracy of position control and force sensing. Indeed, the robot model can be improved with calibrated sensorrobot system, geometric and material parameters. These items are coupled between each other and can be simultaneously calibrated. Compared to rigid robots, the calibration of soft robots is more complex due to the difficulty to model the deformation mechanics of a soft material. The actuators can also be calibrated using embedded force sensors or position encoders in a decoupled manner with respect to other parameters. However, the use of sensors is expensive and sometimes limited by the space. Therefore, calibration of a sensor-robot system and actuators, without using sensors embedded in the actuators, is investigated in this paper. By minimizing the difference between the observed positions of a set of markers on the real robot and their known positions on its FE model, the rigid sensor-robot transformation and then the inputs (forces) of the actuators can be calibrated.

The approach is general and can be employed for robots with complex configurations.

Using external visual observations as a force sensor is an appealing approach which could simplify the robot design and provide a solution for applications where the size and weight constraints have to be considered. Among potential applications, this external system would be especially useful for delicate manipulation tasks with soft grippers. Prior works focused on the external force computation for continuum robot with contacts at the tip [START_REF] Xu | An investigation of the intrinsic force sensing capabilities of continuum robots[END_REF], [START_REF] Black | Parallel continuum robots: Modeling, analysis, and actuation-based force sensing[END_REF]. In this paper, we introduce a method to solve a much more challenging problem: external force sensing in terms of location and intensity at multiple contact points on the soft robot.

This paper thus proposes a cheap and general strategy for calibration and force sensing of soft robots using an RGB-D camera. Calibration and force sensing are achieved using a real-time Finite Element Method (FEM) which has been employed for position control of soft robots [START_REF] Zhang | Kinematic modeling and observer based control of soft robot using real-time finite element method[END_REF], [START_REF] Zhang | Visual servoing control of soft robots based on finite element model[END_REF]. In contrast to our previous work [START_REF] Zhang | Vision-based sensing of external forces acting on soft robots using finite element method[END_REF], the designed external force sensing solution is marker-free and the accuracy is improved. Two main contributions are described: (1) a sequential calibration method based on the FE model of the soft robot to calibrate the sensor-robot system and the initial actuator inputs, using several fiducial markers on the surface of the robot. (2) A marker-free external force sensing strategy is introduced. Using a set of predefined feature points, the locations of potentially multiple external forces exerted on the robot are estimated based on the segmentation of the point cloud data. The corresponding intensities are computed according to the deflection between the point cloud and the FE model at the locations of the feature points and along their normals, assuming quasi-static equilibrium.

This paper is organized as follows. In Section II, the related research on calibration and external force sensing is reviewed. The quasi-static model of the soft robot and the previous marker-based external force sensing technique are introduced in Section III. Section IV presents the method to calibrate the sensor-robot system and the initial actuator inputs. In Section V, the marker-free external force sensing strategy is presented with the description of both the location estimation and intensity computation methods. The experiments on a soft parallel robot is presented in Section VI to validate the proposed techniques. Finally, a conclusion and some future directions are provided in Section VII.

II. RELATED WORK

Calibration of rigid robots has been intensively investigated and most of the strategies focus on calibrating the robot geometrical parameters [START_REF] Elatta | An overview of robot calibration[END_REF] and the robot-sensor system [START_REF] Shah | An overview of robotsensor calibration methods for evaluation of perception systems[END_REF]. A few research works can also be found for the calibration of deformable robots. In [START_REF] Escande | Kinematic calibration of a multisection bionic manipulator[END_REF] and [START_REF] Tan | Simultaneous robot-world, sensor-tip, and kinematics calibration of an underactuated robotic hand with soft fingers[END_REF], the geometrical parameters are calibrated for a continuum robot and a soft gripper respectively based on a constant curvature model. Model-free calibration is investigated in [START_REF] Thérien | Design and calibration of a soft multiple degree of freedom motion sensor system based on dielectric elastomers[END_REF] where a machine learning technique is employed to calibrate a soft motion sensor system. By minimizing the deflection between loading and without loading, the material parameters of a soft object can be calibrated in [START_REF] Wang | Deformation capture and modeling of soft objects[END_REF], [START_REF] Petit | Using physical modeling and rgb-d registration for contact force sensing on deformable objects[END_REF].

Current researches on force sensing for soft objects can be divided into two major categories: direct sensing and indirect sensing. The first category employs embedded sensors to measure the strain directly by placing the sensors at the tip of robotic tools [START_REF] Puangmali | Miniature 3-axis distal force sensor for minimally invasive surgical palpation[END_REF], [START_REF] Kim | Force sensor integrated surgical forceps for minimally invasive robotic surgery[END_REF], around the shaft in a helical layout [START_REF] Xu | Curvature, torsion, and force sensing in continuum robots using helically wrapped fbg sensors[END_REF], or along the arc length [START_REF] Khan | Force sensing in continuum manipulators using fiber bragg grating sensors[END_REF] of the robotic manipulator. The direct sensing approaches generally lead to more accurate estimation of external forces. However, mounting sensors onto or inside the manipulator is not only expensive but also limited by the size of the instrument and by the softness of the considered robot (as sensors have their own stiffness). The idea of indirect sensing is to estimate external forces using the soft robot or the soft object itself as a force sensor. The actuator forces are employed to estimate the end-effector forces for a continuum manipulator [START_REF] Xu | An investigation of the intrinsic force sensing capabilities of continuum robots[END_REF] and a parallel continuum robot [START_REF] Black | Parallel continuum robots: Modeling, analysis, and actuation-based force sensing[END_REF]. There are works about deflection-based force sensing to estimate tip forces. The contact forces can be estimated based on shape detection [START_REF] Back | Catheter contact force estimation from shape detection using a realtime cosserat rod model[END_REF], tip pose measurement [START_REF] Hasanzadeh | Model-based force estimation for intracardiac catheters[END_REF] and shaft curvature [START_REF] Khoshnam | Modeling and estimation of tip contact force for steerable ablation catheters[END_REF].

The work mentioned above are only suitable for tip force estimation. Sensing multiple external forces along the entire material or robot is more challenging because of the estimation of both force intensities and locations. In [START_REF] Bajo | Kinematics-based detection and localization of contacts along multisegment continuum robots[END_REF], kinematicbased contact detection and localization methods for multisegment continuum robots are presented. A general strategy for soft robots with complex configurations is proposed in [START_REF] Zhang | Vision-based sensing of external forces acting on soft robots using finite element method[END_REF] where both the location and the intensity can be estimated based on the deflection of markers. RGB-D cameras have been used to detect the location of external forces for rigid robots [START_REF] Magrini | Estimation of contact forces using a virtual force sensor[END_REF] and hand-object manipulation [START_REF] Pham | Towards force sensing from vision: Observing hand-object interactions to infer manipulation forces[END_REF]. In [START_REF] Petit | Using physical modeling and rgb-d registration for contact force sensing on deformable objects[END_REF], as a close work to ours, a single external force acting on a soft object is computed through the registration of an FE model on point cloud data.

III. BACKGROUND: QUASI-STATIC MODELING AND MARKER-BASED EXTERNAL FORCE SENSING

In this section, we first introduce the Finite Element based quasi-static model for soft robots. A previous marker-based external force sensing technique [START_REF] Zhang | Vision-based sensing of external forces acting on soft robots using finite element method[END_REF] is then briefly reviewed, as an inspiration for the proposed marker-free strategy.

A. Quasi-static modeling

The discrete-time quasi-static equation of a soft robot with arbitrary external loads is deduced from a real-time FE method. By ignoring dynamics, the quasi-static equilibrium function of the soft robot at the (k + 1)th time step is given by:

f ext + F (x) + H T a λ a + H T f λ f = 0 (1)
where x ∈ R n (n is the number of degrees of freedom of all nodes) is the position vector of all the FEM nodes and depends on the configuration of the soft robots. f ext and F (x) represent respectively the external loads (like gravity) and the internal stiffness forces. λ a ∈ R a (a is the number of actuators) and λ f ∈ R f ( f is the number of external forces) are actuator and external forces. H T a ∈ R n×a and H T f ∈ R n× f provide the directions of the forces on the nodes exerted by the actuators and the external forces.

In [START_REF] Rus | Design, fabrication and control of soft robots[END_REF], the internal stiffness forces F (x), which are nonlinear, can be expressed based on first-order Taylor expansion, as:

F (x + dx) ≈ F (x) + K (x) dx (2)
where the tangent stiffness matrix K (x) ∈ R n×n is highly sparse and depends on the actual position of the nodes x. dx is the displacement between consecutive configurations. The equilibrium equation of a soft robot at each time step can be obtained by combining (1) and (2). To reduce its dimension and make real-time computation possible, the equation can be projected into the constraint space by multiplying each term in the equilibrium by both matrices H e and H a , where H e maps the whole set of nodes to a predefined subset, or end-effectors, on which the forces will be effectively computed [START_REF] Coevoet | Software toolkit for modeling, simulation, and control of soft robots[END_REF]. In this work, end-effectors consist in a set of feature points defined on the surface of the robot, as described in section V. Then, position vectors of the end effector δ e ∈ R 3e (e is the number of effectors) and the actuators δ a ∈ R a (a is the number of actuators) can be written respectively as:

δ e = W ea (x) λ a + W e f (x) λ f + δ f ree e (3)
δ a = W aa (x) λ a + W a f (x) λ f + δ f ree a (4)
where the compliance matrices can be computed, based on the configuration of the soft robot, as

W i j (x) = H i K -1 (x) H T j (5)
with i, j standing for the subscript e, a and a, f , respectively.

When the actuator forces and external forces are computed, the configuration of the soft robot is updated by

dx = K (x) -1 H T a λ a + K (x) -1 H T f λ f + dx f ree (6)
Using the compliance matrices, we can get a measure of the mechanical coupling between effectors and actuators (W ea ∈ R e×a ), effector and external forces (W e f ∈ R e× f ), actuators and actuators (W aa ∈ R a×a ), and actuators and external forces (W a f ∈ R a× f ). If the actuators and external forces are applied on different nodes or with different directions, there will be no linear dependency between the lines of H a , H f and H e so that the matrix W aa is positive-definite and W ea ,W e f and W a f has full row rank or full column rank.

B. Marker-based external force sensing

In this section, we briefly describe the strategy used in [START_REF] Zhang | Vision-based sensing of external forces acting on soft robots using finite element method[END_REF] for external force sensing based on the Finite Element Method and markers placed on the robot, which the method proposed here is inspired by. As shown in Fig. 1, the actuator input λ a is sent to both the soft robot and its FE model, so the influence of the actuator inputs can be compensated. When an external force acts on the soft robot, there is a deflection between the FE model and the real robot. The external forces λ f are computed to make the feature points on the FEM model δ e align to their corresponding points on the real robot δ r e .

IV. CALIBRATION

Similarly to [START_REF] Zhang | Vision-based sensing of external forces acting on soft robots using finite element method[END_REF], the external force sensing method consists in exploiting the deviation between the real robot, observed with a vision sensor and its FE model. Therefore, calibrating the two systems is necessary to make sure that this deviation is minimized without external forces and are measured in the same coordinate system. To simplify the calibration method, we assume that both the geometric and material parameters of the soft robot are modeled accurately. The items to be calibrated are thus the rigid transformation between the robot and the vision sensor (the RGB-D camera), in an eye-to-hand manner, and the initial actuator inputs.

A set of feature points are set on the surface of the robot. As an offline procedure, we consider the use of fiducial markers placed on the surface. Their known positions in the robot frame, defined as δ robot , are matched with their observed positions in the camera frame, defined as δ cam . The goal of the calibration technique is then to minimize a position error with respect to the camera/robot rigid transformation (R, T) and the initial actuator input λ a : min

R,T,λ a n ∑ i=1 Rδ i cam + T -δ i robot 2 (7) s.t. λ a ∈ Ω with δ robot = δ 0 robot ...δ n robot T = W ea λ a + δ f ree e .
* 2 is the 2-Norm and Ω is the constraint for the actuator input. n is the number of markers on the robot surface. T ∈ R 3 and R ∈ R 3×3 are respectively the translation vector and the rotation matrix which align each observed marker to their positions in the robot base frame defined for the FE model.

The method consists of two sequential steps: the computation of the optimal rigid transformation and the optimization of the actuator inputs.

The correspondences between two sets of points generate a linear least-squares problem that can be solved robustly using the Singular Value Decomposition (SVD) method. By first setting λ a = 0, the optimal rigid transformation can be obtained by solving [START_REF] Elatta | An overview of robot calibration[END_REF] with respect to T and R.

Based on the estimated optimal rigid transformation R, T , the actuator input is then estimated by solving:

V. EXTERNAL FORCE SENSING

The objective of this section is to describe the method to sense the external forces based on the FE model and using the RGB-D camera. We can distinguish two parts: location recognition based on the segmentation of the point clouds provided by the camera, and intensity computation based on the deflection of a set of preliminarily selected feature points. An acquired point cloud is then filtered through PassThrough, VoxelGrid filters and RadiusOutlier removal, to remove points whose values fall outside a given interval along a specified dimension, to downsample the point cloud and to remove outliers from noisy data, respectively.

2) Segmentation: We assume that the color of the robot is homogeneous and distinct from the ones of the surrounding objects. Using a color-based region growing segmentation technique, the filtered point cloud is segmented into two parts: the point cloud of the soft robot, defined as X r (shown in blue in Fig. 2) and the point cloud of the surrounding objects, defined as X o (shown in red in Fig. 2).

3) Contact location detection: The contact locations between the soft robot and the surrounding objects are estimated by processing both point clouds. The algorithm is summarized in Algorithm 2 and can be described as follows.

The contact points between the robot and the colliding objects are usually difficult to be detected because they can be occluded by the contact operator. When a contact occurs, some points in X o get close to X r . For each point x o in X o , a nearest neighbors search, using kd trees, is then performed among X r . If the number of neighbors is larger than a defined threshold number n neigh , we regard x o as a contact point.

When all the contact points are collected, they are clustered into several groups based on euclidean cluster extraction. The average position x d in each group is computed as the detected location of an external force. Compute the average position End 4) Location mapping: The detected locations are mapped to the surface of the FE model where multiple feature points X e = x e,k P k=0 ⊂ X f e with X f e the set of vertices of the FE model are predefined and evenly distributed on the surface, as shown in Fig. 2. These feature points are employed to approximate the surface of the robot. For each detected location x d , we find the feature point x e,k d on the FE model which is the closest to x d . Force estimation will then be performed on the obtained subset of contact feature points

x e,k d on which the detected contact locations are mapped. 5) Feature points matching: External force sensing requires the computation of the displacements of the feature points X e predefined on the surface of both the soft robot and its FE model. Since these predefined feature points X e on the FE model are not recognizable like fiducial markers, a matching technique between X e and the observed point cloud of the robot X r is proposed, as described in Algorithm 3 and as follows.

For each feature point x e , we perform matching by looking for a correspondence along its normal direction n e which is computed using the surface of FE model (see Fig. 3). Then we find the point x c in X r which has the minimum distance d mini to the normal line. Each feature point x e and the found point x c are regarded as a correspondence, for which the displacement of x e can be computed. We define X c = x c,k P k=0 ⊂ X r as the set of correspondences. In real world application, some feature points may not be available because of occlusions. During matching, the hidden feature points on the FE model need to be detected. Consequently, we predefine a threshold distance d m to discard potential outlier feature points. For each x e , if the distance to its corresponding point in X r is larger than the threshold, x e is not available and its displacement will not be considered for the intensity computation described in the next subsection.

The matching process should also avoid the situation for which the point cloud is not well segmented, e.g. objects with colors similar to the ones of the robot. A threshold distance d l is predefined to detect the matches which fall into this situation. If the distance between a feature point x e and its match x c is larger than d l , the match is considered as an outlier and x e is discarded for force computation.

B. Intensity computation

The external forces at the found contact feature points x e,k d are computed so that on the FE model deforms towards the real robot and its point cloud. By relying on the methodology described in section III.A, we map the quasistatic equation (3) to the motion space along the normal of the robot surface.

δ N e = W N ea λ a + W N e f λ f + δ f ree,N e ( 10
)
where δ N e = x e,0 n T e,0 • • • x e,P n T e,P and δ f ree,N e =

x f ree e,0 n T e,0 • • • x f ree e,P n T e,P . It is indeed easier to capture this direction of motion than other directions, due to ambiguities in the nearest neighbors searches along the tangential plane. For the same number of effectors, this strategy can reduce the constraint size and further reduce the computation time.

The compliance matrices W N ea and W N e f can be obtained using the FE method. Based on the approach described in Fig. 1, the intensity of the external forces at the contact feature points x e,k d can be computed by solving an optimization problem λ = arg minΓ (λ ) where Γ (λ ) is the objective function, with respect to λ f , contributing on the contact feature points:

Γ (λ ) = 1 2 W eI (x) λ I + δ f ree,N e -δ N c 2 ( 11
)
where

W eI = W N ea W N e f , λ I = λ a λ f and δ N c = x c,0 n T e,0 • • • x c,P
n T e,P . The computed external forces λ f and λ a are employed to update the FE model to reach the same configuration of the robot observed by the camera.

In the case of a constrained optimization, a numerical optimization algorithm is necessary to be employed. In order to match the standard quadratic programming (QP) formulation for the usage of software packages [START_REF] Ferreau | qpoases: A parametric active-set algorithm for quadratic programming[END_REF], [START_REF] Thérien | Design and calibration of a soft multiple degree of freedom motion sensor system based on dielectric elastomers[END_REF] can be converted to

min λ 1 2 λ T Q s λ + c T s λ (12) s.t. λ a = λ r a ( 13
)
or

W N a λ + δ f ree,N a = δ r,N a ( 14
)
where Q s = W T eI W eI and c s = W T eI δ f ree,N e δ N c . λ r a and δ r a are respectively the actuator force and actuator displacement sent to both the soft robot and its FE model. The control inputs are considered for the computation by the constraints ( 13) and ( 14). ( 13) and ( 14) are employed respectively when the actuator force and the actuator displacement are chosen as the control input. Remark 1. The dimension of Q s is equal to the number of external forces and actuators so that the computation cost for the QP optimization ( 12) remains almost constant, even if a larger number of feature points is employed for the computation.

VI. EXPERIMENT RESULTS

The strategy proposed in this paper is validated by some experiments described in this section. In the first subsection, the experimental setup is introduced. Then the performances of the calibration and force sensing methods are shown in the following two subsections.

A. Experimental setup

As shown in Fig. 4, the experimental setup consists of three components: a soft parallel robot, a real-time FE model, and an RGB-D camera. The soft robot has four legs and is actuated by cables on each leg so that it is difficult to get an analytical kinematic model. It is made of silicone with the Young modulus 180kPa. The FE model of the soft robot is built with 4884 elements and is simulated in the SOFA framework [START_REF] Faure | Sofa: A multi-model framework for interactive physical simulation[END_REF], which is a real-time FEM simulation software. The methods for FEM simulation and soft robot modeling are described in details in [START_REF] Coevoet | Software toolkit for modeling, simulation, and control of soft robots[END_REF]. The configuration of the soft robot is updated with the frequency of 22 Hz (5 markers) for calibration and 7Hz (51 feature points) for external force sensing. The Realsense depth camera D415 by Intel company is employed to capture the scene. The frequency of the image processing is 10Hz for force sensing.

B. Calibration performances

The calibration technique is validated by both simulation and real experiments. The variables to be calibrated in this paper are R, T and λ a as shown in [START_REF] Elatta | An overview of robot calibration[END_REF].

1) Validation by simulation:

As shown in Fig. 5, two cases with different number and distribution of markers are considered for the validation. In the first case, five markers (the white dots in Fig. 5) are defined. Nine feature points (both black and white dots in Fig. 5) are employed in the second case. The objective of the calibration technique is to actuate the initial configuration (Fig. 5 (a)) with respect to oxyz in order to reach the desired one (Fig. 5 (b)) with respect to pxyz. The calibrated configuration is shown in Fig. 5 (c) with respect to oxyz.

The actual values of all variables for both cases are shown in Table . I. The calibrated values and the residual errors of the objective function [START_REF] Elatta | An overview of robot calibration[END_REF] are listed in Table . I. Being validated by simulation, the proposed calibration strategy can provide calibrated parameters with a high accuracy.

The residual errors with respect to the number of iterations is shown in Fig. 6. The calibration with nine markers has a faster convergence and a higher accuracy so that we use it for the real world experiment. 2) Validation by real world experiment: Instead of using the direct FE model, the real soft robot (Fig. 5 (d),(e)) is employed for validation. Here, we show the validation results using nine markers which are selected and manually measured from the point cloud. Fig. 5 (a) is also the initial configuration of the FE model. Fig. 5 (e) and ( f ) are respectively the configuration of the soft robot with respect to qxyz and the calibrated configuration of the FE model with respect to oxyz.

Table II shows the residual error for the real world experiment. The cable displacement δ a (computed by (4) with λ f = 0) is chosen as the variable for the validation. The initial configuration of the soft robot (Fig. 5 (d)) is employed to get the actual value of R, T. The actual value δ a = δ initial a + δ a where δ initial a is computed by the calibration based on the initial configuration of the soft robot (Fig. 5 (d)) and δ a is randomly set by users. Actuated with δ a , the configuration of soft robot is shown in Fig. 5 (e).

As shown in Fig. 6, the residual error can be reduced to 7.47 mm after ten iterations in our experiment. The residual error is larger than errors in simulation. This is due to errors in the FE model, in the image processing step and in the correspondences between the artificial markers and the feature points defined on the FE model.

The calibration method has several limitations: (1) A

C. External force sensing performances

Given the accurate initial configuration of the FE model provided by the calibration step, the external force sensing can be handled. In this subsection, we show the performances of image processing, external force computation and the compound of location detection and intensity computation.

1) Performance of image processing: The image processing step in this paper is based on some algorithms provided in the Point Cloud Library (PCL) [START_REF] Rusu | 3d is here: Point cloud library (pcl)[END_REF]. The objective is to segment the point cloud of the robot and the one of its surrounding environment. The color-based region growing segmentation technique is employed, with the assumption that the color of the robot is homogeneous and is different with respect to its surrounding. As shown in Fig. 7, the contact points can be detected successfully with the blue points being the surface of the robot and the yellow points being the contacts location. 2) Performance of external force computation: Multiple three-dimensional external forces can be computed and tracked using the proposed methods. But in this subsection, we focus on one-dimensional force computation. As shown in Fig. 8, the external force generated by a weight in z-direction which is along the direction of gravity is applied on the tip of the robot. The control inputs are also considered with constant cable displacements. Based on the inverse FE model, the intensity of the weight is computed and is listed in Table .  III for different weight. The Young's Modulus is set to be 180 kPa which is calibrated based on the relation between weight and deformation. From Table III, the computed intensity can reach a higher level of accuracy where the average error for six weights is around 4%. 3) External force sensing: In Fig. 9, the screenshots of the external force sensing system is shown with the integration of location estimation and intensity computation. In the experiment, we consider two cases: the soft robot with one external force ((a) and (c)) and two external forces ((b) and (d)). 51 feature points are pre-defined on the surface of FE model and are regarded as the possible locations of external forces. When the locations of the external forces are detected based on the image processing of the point cloud, the correct locations on the FE model are selected. Then the intensities are computed by solving the optimization problem described in section V.B so that the FE model can be actuated to align to the corresponding point cloud.

The limitations of the marker-free force sensing approach are: (1) We employ a color-based image segmentation technique for the point cloud which reduces the generalization and is sensitive to the color contrast between the considered areas;

(2) The external forces are assumed to be point-wise forces which are applied on the surface of the soft robot. Therefore, the estimation accuracy reduces if the contact area between the robot and it environment is larger; (3) The implementation needs the visibility of the contact points by the camera.

VII. CONCLUSION

This paper proposes a strategy for calibration and external force sensing for soft robots. By choosing several corresponding feature points on the surface of a soft robot and its FE model, we can calibrate the sensor-robot system and the initial actuator input. The calibration is based on the numerical model of the soft robot and can be employed for robots with complex structures. The marker-free external force sensing technique relies on matching the point cloud provided by an RGB-D camera and predefined feature points on the surface of the FE model. Using the external force sensing strategy, both the location and the intensity of external forces on the entire surface can be estimated. This method couldï¿oebe usedï¿oe in more general computer vision applications in-volvingï¿oedeformable objects and requiring external force estimation (for instance for robotic manipulation, surgery, augmented reality and motion capture problems).

In our future work, we would like to extend the strategy for the calibration of geometric and material parameters. In order to improve the performances of external force sensing, we would like to employ a more robust approach for point cloud segmentation and to use a multi-camera system to capture the whole shape of the soft robot and its contacts. GPU programming would be employed for the simulation and image processing steps to increase the response speed of the force sensing algorithm. We also plan to extend the system to the hybrid position/force control based on the FE model. Besides, we would like to evaluate the proposed techniques via concrete real world applications.

Fig. 1 .

 1 Fig. 1. Framework of external force sensing for soft robots. EFC is the algorithm of external force computation. λ a is the actuator input for both the soft robot and its FE model. λ r f and δ r e are respectively are the external force and position of effectors for the soft robot. λ f and δ e are the corresponding variables for FE model.

min λ a 1 2 W

 2 ea λ a + δ f ree e δ tran 2 (8) s.t. λ a ∈ Ω with δ tran = Rδ i cam + T. It can be converted to a standard quadratic programming (QP) formulation: min Qλ a + c T λ a (9) s.t. λ a ∈ Ω where Q = W T ea W ea and c = W T ea δ f ree e δ tran . The calibration algorithm is also reported in Algorithm 1. In (7), W e and δ f ree e cannot be pre-computed based on the initial configuration of the FE model. Therefore, after the computation of λ a , the configuration of the FE model should be updated with I 2 iterations in order to compute more accurate values for W e and δ f ree e . The calibration stops after I 1 iterations. Algorithm 1 Calibration algorithm 1. Place markers on the robot, with corresponding positions on the FE model 2. Read the positions of markers from the vision sensor 3. While Iteration < I 1 Compute positions of the markers on the FE model frame with (3) Compute rigid transformation R, T Compute positions of the markers in the camera frame While Iteration < I 2 Compute actuator forces λ a (9) Update the configuration of FE model with (6) 4. Output T, R and λ a

  A. Location recognition and feature points matching 1) Point cloud filtering: The surfaces of both the soft robot and the surrounding objects (the manipulation tools or an obstacle for instance) are captured by the RGB-D camera.

Fig. 2 .

 2 Fig. 2. Location recognition and feature points matching. The image processing method consists of five steps: filtering, segmentation, location recognition, location mapping and feature points matching.

Algorithm 3

 3 Feature point matching 1. Define feature points on the surface of FE model and two threshold distance d m and d l 2. For each feature point x e Compute the normal direction For each point in robot point cloud Compute distance between x e to normal line End Compute the minimum distance d mini If d mini > d m , the feature point is hidden Else possible match x c obtained Compute distance d between x e to x c If d < d l match x c obtained End

Fig. 3 .

 3 Fig. 3. Matching process of the feature points along the normal direction. The black, green, gray, orange and red points are respectively the robot point cloud, effector point cloud, outliers after segmentation, matches in the outliers and the matches for effectors. d m and d l are the predefined threshold values to remove imprecise matches in the segmented point cloud.

Fig. 4 .

 4 Fig. 4. Experimental setup for calibration and external force sensing of soft robot. The setup consists of an RGB-D camera (a) which generates the point cloud of robot surface (d), a soft parallel robot (b), a real-time FE model (c).

Fig. 5 .

 5 Fig. 5. Validation design for the calibration by simulation ((a), (b) and (c)) and real experiment ((d), (e) and ( f )). (a), (b) and (c) are respectively the initial, the desired and the calibrated (based on (b)) configurations of the FE model with respect to oxyz, pxyz and oxyz. (d) and (e) are respectively the initial and the desired configurations of soft robot with respective to qxyz. ( f ) is the calibrated (based on (e)) configurations of the FE model with respect to oxyz. The spots (white and black) are the markers on the FE model.

Fig. 6 .

 6 Fig. 6. Residual error of the validation with respect to the number of iterations by simulation and real world experiment.

Fig. 7 .

 7 Fig. 7. Screenshot of image processing results on the location of external forces. The yellow points are the detected locations of external forces. (a) and (c) are the situation of one external forces. (b) and (d) correspond to the detection of two external forces.

Fig. 8 .

 8 Fig. 8. Experiment setup for the validation of intensity computation. (a)the soft robot with a weight hanging on the tip; (b)the FE model with feature points and computed force

Fig. 9 .

 9 Fig. 9. Screenshot of external force sensing. The robot has four cables with constant length for the experiment. (a) and (c) show one external force on the actuated soft robot. (b) and (d) show case with two external forces.

  Algorithm 2 Contact location detection 1. Define a constant search radius r search and a threshold number of neighbors n neigh 2. If objects are found after segmentation For each point in contact point cloud Search neighbors (r search ) in robot point cloud If number of neighbors larger than n neigh Feature points detected End Separate the detected points into groups For each group

TABLE I CALIBRATION

 I RESULTS USING SIMULATION. THE UNIT OF λ IS NEWTON. THE UNIT OF BOTH T AND THE RESIDUAL ERROR IS MILLIMETER. AV: ACTUAL VALUE; RE: RESIDUAL ERROR; (I): THE VALIDATION WITH FIVE MARKERS; (II) THE VALIDATION WITH NINE MARKERS.

	Items	R		T	λ
	AV	-0.147 -0.485 -0.862 -0.273 0.941 0.200	0.305 -0.851 0.427	-264.956 263.885 12.767	1.200 1.000 1.000 1.200
	RE(I) 0.025	-0.147 -0.484 -0.862 -0.273 0.941 0.200	0.305 -0.852 0.426	-264.965 263.913 12.947	1.199 1.000 0.999 1.200
	RE(II) 0.024	-0.147 -0.485 -0.862 -0.273 0.941 0.200	0.305 -0.851 0.427	-264.938 263.899 12.799	1.200 1.001 1.001 1.201

TABLE II CALIBRATION

 II RESULTS USING THE REAL WORLD EXPERIMENT. THE UNIT OF T, δ a , AND THE RESIDUAL ERROR IS MILLIMETER. AV: ACTUAL VALUE; RE: RESIDUAL ERROR; (II) THE VALIDATION WITH NINE

			MARKERS.
	Items		R	T	δ a
	AV	-0.732 0.681 0.000	0.679 0.729 -0.087 -0.996 -0.059 -0.064	17.480 12.598 566.918	3.947 5.176 7.920 7.644
	RE(II) 7.47	-0.736 0.677 0.018	0.674 0.735 -0.069 -0.997 -0.060 -0.039	17.338 -1.566 570.349	3.249 5.065 10.921 8.904

TABLE III INTENSITY

 III COMPUTATION. THE FORCE UNIT IS mN.

	Actual force	260	460	660	760	860	1060
	Computed forces	230	434	634	756	873	1045
	Absolute error	30	26	26	4	7	15
	Computation error	11.5%	5.7%	3.9%	0.5%	1.5%	1.4%

This work was supported by the Region Hauts-de-France, the project COMOROS (ERDF funds).

The authors are with