Navigation and 3D Surface Reconstruction from Passive
Whisker Sensing

The sensing method presented here builds upon prior work on perception of unstructured environments through contacts Petrovskaya and Khatib (2011); Bohg et al. (2010); Wang et al. (2020); Koval et al. (2015); Manuelli and Tedrake (2016); Suresh et al. (2021); Lin et al. (2021). A common challenge when sensing free-standing objects is that the act of contact sensing often will change the state of the object. For objects of known shapes, Koval et al. developed a Particle Filtering approach to estimate object location through a sequence of pushes to collapse the belief distribution Koval et al. (2015). Suresh et al. expanded on this work by posing the problem as a Simultaneous Localization And Mapping (SLAM) problem and were able to estimate both shape and location of objects Suresh et al. (2021). While these methods work when interacting with isolated objects, the pushing approach is more challenging when an object is amidst clutter which constrains both the object and robot arm.

Sensing through non-intrusive contacts has the advantage of objects remaining static, making state estimation easier. The most common of such perception methods is vision, however, RGB-D cameras are not well suited for close range sensing as is typically necessary when reaching into confined spaces with objects. Some work has investigated using close range proximity sensing Hsiao et al. (2009); Tsuji and Kohama (2019); Schlegl et al. (2013), but these methods do not perform well when sensing specular or transparent surfaces for optical transducers, or may be susceptible to variations in materials properties for magnetic and capacitive transducers. Sensing through mechanical contact is not affected by these problems.

Some investigations have addressed whisker- or antenna-based sensing in robotics. Early work by Kaneko et al. showed a method of active probing where a flexible antenna is rotated by actuators at one end to make contact with objects while estimating contact location with measured rotational compliance Kaneko et al. (1998). Subsequent efforts have addressed improved contact localization for objects of varying shapes Solomon and Hartmann (2010); Kim and Möller (2007); Merker et al. (2021); Gomez et al. (2024, 2023). Using 3-DOF force/torque sensing at the base (two bending torques and one axial force) it is possible to deduce contact locations from single measurements Sofla et al. (2023, 2024); Huet et al. (2017); Emnett et al. (2018); Nguyen et al. (2020). However, these methods require either solving a nonlinear system of differential equations or providing a unique mapping of transducer signals to contact locations, leading to computational complexity and limited resolution under realistic conditions when an arm is moving among small and light objects.

For the task of reaching in an unstructured space, an advantage of using non-obtrusive sensing is that it can be directly used for collision-free navigation. Typically, collision avoidance navigation in clutter is done by sensing through rigid or soft collisions Saund et al. (2019); SaLoutos et al. (2023); Gruebele et al. (2020, 2021), model-based control Jain et al. (2013) and path planning strategies Muhayyuddin et al. (2018). However, relying on forceful collisions for navigation can be challenging as the safety of the solution depends on speed of robot motion, sensing, control latency, and compliance of the system. Using optical or acoustic proximity sensing has robustness limitations to object translucency and texture as mentioned in Section 2.2. Using highly compliant structures to sense surrounding spaces, on the other hand, overcomes these limitations while effectively still achieving a safe interaction with surrounding objects.

When non-zero deflection is measured by our sensors, it is a clear indication of the presence of an object within the sensing range of the whisker (Figure 2). One implementation of a reactive controller can be to apply an artificial force at the sensor location in the direction counter to the normal, such the robot will move and avoid contact with the sensors attached. While this is a safe approach, we gain little information about the surroundings by fully avoiding contact with the whiskers. Instead, we are interested in allowing for sustained contacts with a safety boundary such that the controller will prevent objects from getting too close to the surface of the robot (Figure 1). This approach affords more information gain by processing the contact location for shape reconstruction as will be shown in Section 5, while also avoiding collisions with objects.

We implemented a reactive collision avoidance controller using a admittance-type control (similar to that found in Lynch and Park (2017)):

where $x$ is the end-effector Cartesian pose, $x_{g}$ is the goal pose, $\prescript{B}{E}{J^{T}_{i}}$ is the Jacobian that relates forces at the i-th sensor reference frame to forces and moments at the end-effector frame, $M_{z},i$ is the deflection measured at the i-th sensor (in this case measured in sensor counts), $M_{z,th}$ is a user defined threshold for measured deflection at which artificial forces start acting, finally $\gamma$ is a scalar found empirically that relates these deflections to the rate at which the controller moves away from the penetrating surface.

We implemented this reactive controller on the system shown in Figure 1 comprised of a Flexiv Rizon robot arm equipped with an end-effector covered with an array of 16 whisker sensors. We controlled the robot to move among a clutter of objects at moderate speeds (4 cm/s). We tested this controller under three different scenarios:

1. 1.

   Scenario 1: Robot moves at full speed towards a wine glass.

2. 2.

   Scenario 2: Robot is teleoperated in arbitrary motions into collision with a clutter of objects.

3. 3.

   Scenario 3: Robot is commanded to move with a constant velocity vector towards a clutter of objects with an opening of free-space.

Success was measured as not toppling objects over or shifting them from their resting position. We performed 5 experiment trials using Scenario 1, where the robot was commended at a speed of 4 cm/s towards a wine glass. In all 5 trials, the controller was able to push the robot away from the collision upon sensing contact with the whiskers. For Scenario 2, we used a joystick to teleoperate the motion of the robot intentionally moving towards collision with a clutter of objects. Throughout the experiment, which lasted for 40 seconds, the controller was able to avoid all hard collisions. We performed 4 trials using Scenario 3 with different object arrangements. Light object collisions were observed in two trials but caused no toppling or appreciable object shifting, and in all four experiments the controller was able to guide the robot through the free-space opening among objects. The result of this collision avoidance behavior is best seen in the accompanying video  ¹¹1Reactive collision avoidance experiments videohttps://youtu.be/Rvx4tWSkfu8.

We begin by summarizing the assumptions made to limit the scope of the problem.

1. 1.

   Objects that come into contact are immobile in the world reference frame. In other words, if the robot does not move then contact locations do not change.

2. 2.

   There is at most one contact point on a whisker with the environment at any time. This will be true for convex objects.

3. 3.

   Frictional forces along the whisker are negligible in terms of their effect on the sensor. This again will generally be true given the nitinol whisker material.

In the following derivation we follow the conventions in Murray et al. (1994) with lowercase boldface fonts for vectors, uppercase boldface fonts for matrices and $\{F\}$ denoting a reference frame. Lowercase superscript and subscript letters indicate the reference frames in which a vector or matrix is expressed. For example, $\bm{v}^{a}_{ab}$ is the linear velocity of reference frame A relative to B (subscript ${ab}$) as viewed in reference frame A (superscript $a$).

Let $\{B\}$ be the reference frame of the sensor base and $\{S\}$ be the spatial (world-fixed) reference frame. We define our process and sensor model as follows

Where $\bm{x}_{k}=\bm{p}_{c}$ is the position vector of the contact point relative to the origin of $\{B\}$ at time-step $k$. $y_{k}$ is the sensor measurement at time-step $k$. $\bm{v}^{b}_{sb}$ and $\bm{\omega}^{b}_{sb}$ are the linear and angular velocity, respectively, of sensor base $\{B\}$ relative to world-fixed frame $\{S\}$ as viewed in the body frame. Process and sensor noise are modelled as Gaussian white noise where $\bm{w}_{k},\bm{\nu}_{k}\sim\mathcal{N}(0,\,\sigma^{2})$. $g:\mathbb{R}^{2}\to\mathbb{R}$ is our sensor model that maps contact location to moment measurements which we will elaborate on in Section 5.2.

To define the process model, we first find the velocity of point $p_{c}$ relative to $\{S\}$ as viewed in the body frame $\{B\}$ denoted as $\bm{v}^{b}_{sp_{c}}$. Let’s consider $p_{c}$ as a point with respect to $\{B\}$, and define a point $b_{p}$ that is instantaneously coincident to $p_{c}$, but fixed in $\{B\}$. We can find the linear velocity of $p_{c}$ with respect to the spatial frame as

where $\bm{v}^{b}_{sb_{p}}$ is the linear velocity of point $b_{p}$ relative to $\{S\}$ as viewed in the body frame, $\bm{v}^{b}_{sb}$ is the linear velocity of $\{B\}$ relative to $\{S\}$ as viewed in the body frame, $\bm{\omega}^{b}_{sb}$ is the angular velocity of $\{B\}$ with respect to $\{S\}$. $\bm{p}_{c}$ is the position vector of point $p_{c}$ relative to $\{B\}$’s origin.

Given our assumption that the contact point remains static in the spatial frame (i.e. $\bm{v}^{b}_{sp_{c}}=\bm{0}$), the contact point velocity relative to $\{B\}$ is

where $[\bm{p}_{c}]$ is the skew-symmetric matrix of vector $\bm{p}_{c}$ and $\bm{I}$ is the identity matrix.

As the sensor is attached on a robot link, $\bm{v}^{b}_{sb}$ and $\bm{\omega}^{b}_{sb}$ can be found through

where $\bm{J}_{sb}^{b}$ is the body Jacobian of $\{B\}$ relative to $\{S\}$, and q is the vector of manipulator joint angles.

With the velocity of the contact point with respect to $\{B\}$ defined, we can express the process model as

which gives as $\bm{A}=I$ and $\bm{B}=-\delta_{t}\begin{bmatrix}\bm{I}&[\bm{p}_{c}]\end{bmatrix}$ in Equation 4.

In the case when contacts are not static, such as when they travel along objects having a large radius of curvature, the velocity of the contact point relative to the world $\bm{v}^{b}_{sp_{c}}$ is non-zero. However, since we assume no prior knowledge of the object shape and location, the contact velocity is not predictable. As inferences are executed very fast (250 Hz) contact points do move far between iterations, so we choose to make the static assumption and correct for errors using the sensor model.

In order to find the sensor model, $g_{x}$ and $g_{y}$, which maps contact point locations, $\bm{x}_{k}$, to predicted moment measurements, $\bm{y}_{k}$, we develop a calibration platform and procedure. To gather data for this mapping we used a 3-axis calibration stage shown in Figure 4: model LSM100A from Zaber (position resolution of $0.1\mu$m). A calibration rig is attached at the end of the stage which has a V-shaped groove used to enforce a known contact location on the whisker. During data collection, position and magnetic sensor data are recorded as the end-effector of the stage is driven to deflect the whisker with the calibration rig. This procedure is then repeated for different positions of the stage by tracing an arbitrary trajectory (as shown on the right of Figure 4) that spans the sensing region.

From calibration we obtain a dataset $\mathcal{D}=\{(\bm{x}_{i},\bm{y}_{i})\mid i=1,...,n\}$ where $\bm{x}_{i}$ are 3D positions of the contact location and $\bm{y}_{i}$ is the bending moment measurements from the Hall effect sensor in x and y axes as illustrated previously in Figure 3.

We use the method of Gaussian Process Regression (GPR) to find a model that will best fit the calibration dataset that maps three-dimensional position input to two-dimensional sensor response output. We use GPR as it can yield highly expressive models and works well for noisy observations.

The underlying function that relates the data points is a kernel function. Many alternative kernel functions also exist Williams and Rasmussen (2006), and in practice, kernels are chosen through empirical results or through prior knowledge about the underlying distribution. The most frequently used kernel function is the Squared Exponential kernel, also known as the Radial Basis Function (RBF). In this work, we found that a Thin-plate (TP) kernel yielded a sensor model that is more robust to tracking divergence compared to models obtained through the RBF kernel. The two kernels are defined as follows:

where $l$ is a hyperparameter that tunes the locality of the RBF (the higher $l$ is, the less influence data points have on farther neighbors), and $R$ is the radius of the region within which the TP model will minimize the second-order gradient.

One notable difference between models from a TP kernel and RBF kernel is that at the extrapolated region (i.e., the domain outside of where data was collected) the TP kernel maintains a mean with a constant slope whereas RBF has a mean that converges to zero Williams and Fitzgibbon (2006). This can be visualized in Figure 5, where we show a surface plot of a slice of the model at $z=0$. From the RBF models we can see that the mean value converges to zero exponentially at a distance away from data points, whereas, TP models have a constant slope that extends from the edges of the data cluster. This is because the TP kernel is formulated to minimize the second-order gradient Williams and Fitzgibbon (2006).

Models from both kernels fit the dataset very well. The R-squared values for RBF (with hyper-parameter $l=5$) are $0.99815$ and $0.99770$ for $M_{x}$ and $M_{y}$, respectively, and values for TP (with hyper-parameter $R=100$) are $0.99982$ and $0.99969$ for $M_{x}$ and $M_{y}$, respectively. However, it is more advantageous to use the TP models because their extrapolation behavior yields a more stable tracking. This will be further elaborated in  Section 5.3.

As mentioned in Section 5.2, models obtained from the TP kernel are favorable for contact localization. An example of this is illustrated in Figure 6 where we perform a contact localization experiment with a semi-curved sensor (as illustrated in Figure 2) brushing against a cylinder object. We run the tracking algorithm presented in the previous section with the same data and initial conditions, but in one case using the RBF sensor model and the other case using the TP sensor model. In the top two plots we show, overlaid on the sensor models, the goal contact location in green and the UKF sigma points in red. The initialization or prior is such that the sigma points lie in the extrapolated region. It can be observed that, in the RBF model case, the gradients of the region covered by the sigma points are much flatter and the gradient direction might not provide a good correction during the Measurement Update step Wan and Van Der Merwe (2000). In comparison, the gradients in the TP model case are more directed and pronounced in the extrapolated region, making it faster in the Update step to converge towards the correct state. The two bottom plots in Figure 6 then show the different results where the RBF model diverges and TP model converges quickly even though both have the same initial condition and measurement data.

Apart from the empirical data showing that the TP model provides more stable tracking, another reason to use the TP model is that its extrapolated data make more sense than for RBF. The reason the calibration data points are bounded to a region is that the whisker sensor has a finite length, and large deflections cause the calibration rig to break contact with the sensor wire. However, if the whisker had infinite length then we would expect the sensing range to be extended expecting the sensor signal to continue the trend at the boundaries. Given this behavior, the TP model with a constant slope away from the boundaries is a more accurate approximation compared to the RBF model that converges to a zero mean.

In summary, the steps for localization are:

1. 1.

   Make an initial guess $\mu_{0}$ and $\Sigma_{0}$

2. 2.

   Process proprioception data to perform a prediction step as in Equation 4.

3. 3.

   Correct for state distribution based on the sensor prediction.

To track contact locations from a sequence of base moment measurements, we use Bayesian filtering in a recursive algorithm that infers the state distribution from a history of sensor data and control inputs. The posterior distribution is defined as

where $\eta$ is a normalization factor, $b(\bm{x}_{t-1})$ is the prior distribution, and $p(\bm{y}_{t}\mid\bm{x}_{t},\bm{u}_{t})$ and $p(\bm{x}_{t}\mid\bm{x}_{t-1},\bm{u}_{t})$ can be obtained from the sensor and process model, respectively.

We implemented three different non-linear Bayesian filters to compare their performance: Extended Kalman Filter (EKF), Unscented Kalman Filter (UKF) and Particle Filter (PF). A process noise covariance of $1e^{-5}I_{2}$ was empirically found to work well for EKF and UKF. This low noise is reasonable given the accuracy of the optical encoders and calibration system. The sensor noise variance was chosen to be 0.25 which was found through the RMSE of the calibration results reported previously. The process and sensor noise covariance for PF $1e^{-3}I_{2}$ and 1 respectively, which were also found empirically to track with low error. We used N=1000 particles for PF.

As mentioned previously, when the actual contact location travels along the surface of an object, our process model described in Section 5.1 will be inaccurate, which may lead to divergence of the estimate. We cannot model this behavior but we can use known techniques for compensating for model errors. These include adding fictitious process noise or using a Fading Memory (FM) filter (Simon, 2006, p. 139). Both are used to increase the predicted covariance but do so differently. FM scales the prior covariance while fictitious process noise adds a constant positive variance to the diagonals. FM was empirically found to produce better results in our application. We implemented this method on the Bayesian filters by scaling the prior covariance by a factor $\alpha=1.004$ at every time step.

In addition to implementing these filters, we compared tracking performance to a baseline by Solomon et al. Solomon and Hartmann (2010). Similar to our method, this baseline estimates contact points at every time step but with estimates that are deterministic rather than probabilistic (i.e., single values rather than a distribution). Another distinction between our method and this baseline is that our sensor model is calibrated, while the baseline uses a small deflection Euler-Bernoulli beam model. In our implementation of this baseline, we used our calibrated sensor model to obtain predicted moments ($M_{i+1}$) and the arc-length to torque ratio ($\frac{ds}{M_{i}}$) which are used for the estimation correction step. We compare our localization methods to the baseline only in the straight whisker case, as the baseline assumes a nominally straight beam and it would take significant effort to adapt it to an arbitrary shaped beam. For more detail on the baseline please refer to Solomon and Hartmann (2010).

As detailed in Section 5.4, the localization method is a recursive algorithm, and how quickly it converges to the ground truth depends on how good the prior distribution is. Since each interaction between whisker and objects can be brief, it is important that the algorithm can converge quickly.

We do not assume we have any prior information about the world, so it is only possible to guess where contacts will happen on the whisker sensor. However, as information is collected from the environment, we can integrate this data into a map that can then be used to make a more accurate guess on the prior. To build this map we use Bayesian Hilbert Maps (BHM) developed by Senanayake et al. Senanayake and Ramos (2017) which is a method for building continuous occupancy maps in static environments. Two types of data are collected: (i) occupied points which are the mean estimates from the contact localization method and (ii) unoccupied points which are randomly sampled from within the volume occupied by the rigid end-effector link, since we know these regions cannot be occupied by objects. BHM uses a kernel method to calculate the influence of these data points to a grid of hinge points (3D points fixed in the world) by optimizing for a set of parameters $\bm{w}$. Then these sets of parameters and hinge points are used to calculate the posterior occupancy distribution.

The occupancy map can be used to query the likelihood of a point being occupied. For example the probability of a point $\bm{x}$ being occupied $y=1$ is Senanayake and Ramos (2017)

where $\bm{x}$ is the point being queried and $\phi(\cdot)$ is the feature vector defined as $\phi(\bm{(}x))=(1,k(x,\tilde{x}_{1}),k(x,\tilde{x}_{2}),...)$ computed between the query point and all the hinge points, and $\sigma$ is the Sigmoid function.

In order to use an occupancy map to find a localization prior, we first define a set of query points $\bm{X}_{q}=(\bm{x}_{q,1},\bm{x}_{q,2},...)$ which are evenly distributed along the whisker wire as illustrated in Figure 7. At the moment first contact is detected and the contact localization algorithm is being initialized, we can query the occupancy map at each of the points in $\bm{X}_{q}$ and choose the one with the highest likelihood of being occupied. Formally, this is defined as

The design rationale for this algorithm is that, given that we assume a unique contact point, the query point with the highest likelihood of being occupied should be the most likely initial state.

We tested this algorithm in an experiment were a whisker sensor brushed past a 30 mm diameter cylinder multiple times. On the first pass, we make a initial guess that is approximately 2 cm away from the ground truth, as can be seen in the blue plots in Figure 8. On each subsequent pass, we use the occupancy map generated using the previous pass to query for the most likely initial state. It can be seen from the plots that the second and third pass have a much smaller initial error and lead to immediate convergence to the ground truth. This experiment shows that using occupancy maps to initialize the localization algorithm can significantly improve results.

We evaluate the accuracy of our 3D contact localization method through experiments where a sensor is used to brush past an object at different heights scanning the shape of the surface. We perform this experiment on four objects of arbitrary shapes that are 3D printed with a FormLabs Form 3 printer at high resolution and positioned with an optical table relative to the sensor such that we know the shape and location of the object surface. A semi-curved sensor (i.e., with an initial straight section and curved tip, as in Figure 7) is used for scanning and the raw data is processed using UKF and a Thin-Plate sensor model, as described in the previous sections.

Results are shown in Figure 9 where the 3D contact trajectories are plotted together with object meshes (fine resolution) in the world reference frame. We emphasize that there is no manual alignment of trajectories to meshes, but each of them is plotted in their absolute position. It can be seen from all the plots that the scanned data matches well with the object surface. We measured the error for each estimated contact location as its distance from the closest mesh vertex. This error for the Cone object was on average 0.48 mm (S.D. 0.15). As a scaling factor, we can define $\lambda$ as the length of a whisker (55 mm in the present case). Thus the normalized error is $0.009\lambda$. For the Cup object it was on average 0.71 mm (S.D. 0.51) or $0.013\lambda$. For the Rectangular Plate the error was on average 0.28 mm (S.D. 0.18) or $0.005\lambda$. For the Rounded Squares the error was on average 0.47 mm (S.D. 0.4) or $0.009\lambda$. Note that for the Cone and Cup objects it is evident that the whisker is deflected out of its principal plane of curvature, causing the contact to move in the z direction. This highlights the importance of developing a 3D localization method rather than 2D, even for motions that are predominantly in a single plane.

We ran the same object scanning experiment with four arbitrary objects that can typically be found in a home setting. Scanning results are shown in Figure 10. While we do not have information about the exact shape and location of these objects in the world, it can be observed from the 3D plots that the traces resemble the corresponding shapes of the object surfaces.

An advantage of our localization method and formulation is that it is relatively agnostic to the intrinsic undeflected whisker shape. As long as we can get calibration data to produce a sensor model, it is possible to run a Bayesian Filter. This is in contrast to prior methods that need an explicit model that assumes knowledge of the shape of the whisker Solomon and Hartmann (2010); Nguyen et al. (2020); Zhang et al. (2022). The advantage of our method is that it allows flexibility of the method to be tailored to different applications depending on sensing coverage needs. Also, our method is less susceptible to systematic errors introduced during fabrication since we perform a calibration step. To validate that our algorithm performs well for different sensor designs, we performed a simple experiment where sensors of different shapes (straight, curved and semi-curved) brush passed a single contact point and we evaluate how well can localization converge to the ground truth. The initial state estimate is purposefully placed 1 cm away from the ground truth. Data from 10 trials was collected for each sensor design.

From the results shown in Figure 11 we can observe that tracking converges to within 1 mm of the ground truth location in all cases. Convergence happens at different rates which might be due to the different sensor models which produce different cross-correlation matrices. This affects how large a correction step is taken in the update step as detailed in Section 5.4.

In this section we demonstrate the integration of arrays of whisker sensors with an industrial robot arm in a task of reaching among objects of unknown location and shape while using our sensors and algorithms to continuously localize contacts to map the scene. The sensor system consists of 16 semi-curved sensors arranged as shown in Figure 12A, where each sensor is individually calibrated. The scene setup consists of a table with a collection of typical kitchen objects arranged arbitrarily. Among these objects, there are wine glasses, spice bottles, an empty vanilla extract bottle (60 grams), and coffee mugs.

The robot arm is controlled through a joystick to move among the clutter of objects such that the whiskers on both sides brush against the objects’ surfaces. During this execution we record the pose of the end-effector and the sensor readings at a rate of 250 Hz. The data is then post-processed offline.

We used UKF to perform the contact localization with the Thin-Plate model for each sensor. The localization algorithm starts running each time contact is detected from a state of no-contact. After a sensor loses contact, the Bayesian Filter initialization parameters are reset.

Our sensors can accurately trace the surface of objects as shown in Figure 12B; however, these traces can still be too sparse to form a good picture of what the environment looks like. To interpolate this data, we use Occupancy Maps (OM) which were introduced in Section 5.5. Since we expect the environments the robot rummages in to present a degree of regularity – surfaces have low spatial resolution relative to the end-effector size and objects have symmetry in shape – it is reasonable to fill in the spaces in between curves through interpolation. Most free-standing objects in our homes such as spice jars or cans are nearly flat in the vertical direction, so this prior can be used to better interpolate contact data.

Like Gaussian Process Regression, Bayesian Hilbert Maps (BHM) also use the kernel method for related data points. Using the Squared Exponential kernel

we can tune the smoothing parameter $\gamma$ to change the amount of relevance neighboring data points have on each other. It is also possible to choose a kernel that can incorporate prior knowledge that the length-scales of objects are larger in the vertical axis as compared to the horizontal axis (i.e., our objects’ shapes tend to be vary less in the vertical direction). Such an alternative kernel is known as the Automatic Relevance Determination (ARD) RBF Williams and Rasmussen (2006), which is similar to RBF but weights the distance function with different length-scales in each dimension $\mathbf{\omega}^{T}(x-\tilde{x})$. We will refer to this kernel as an Elliptical Basis Function (EBF) as it is similar to a Radial Basis Function, but we use different length-scales in each dimension. The kernel is defined as follows

where $\mathbf{\omega}=\begin{bmatrix}w_{x}\\ w_{y}\\ w_{z}\end{bmatrix}$ is the weight vector where each value is the reciprocal of the length-scale of its corresponding dimension. Since $x$ and $\tilde{x}$ are defined in the world reference frame, we can choose the length scales corresponding to the vertical dimension to be higher. This would have a larger smoothing effect in the vertical direction compared to the lateral directions.

Results of the OM method are shown in Figure 12. Figure 12B shows the scatter plot of contact localization results obtained from the whisker sensors that made contact together with the unoccupied points that were randomly sampled from within the end-effector body with position estimation accuracy of 7.9 mm. It is pertinent to note that the observed differences are primarily attributed to discrepancies between the physical placement of objects within the experimental setup and their manual delineation on the Occupancy Map. These two inputs are used to fit the BHM parameters $\omega$ to predict the occupancy of each point in space as shown in Equation 16. To visualize the OM we sampled 3D space at intervals of 2 mm in all axes, and then plot the voxels that have a probability of being occupied of 0.9 or greater. This result is shown in Figure 12C, where we can recognize all 7 objects (labeled with numbers on the photo), which are those the sensors make contact with, appearing in the reconstruction. To highlight some features that show up in the reconstruction in the Top View plot, the two coffee mugs (#3 and #6) are clearly shown as the two curved surfaces. The flat face of the brown spice bottle #1 shows up as a flat surface. Sensors only make contact with the corner of spice bottle #5 and empty plastic bottle #4, so only a small portion of these items show up in the map. Finally, the tall wine glass #7 is only brushed at the stem, which makes it appear as a cylinder with small radius.

Figure 13 shows an example of the surface reconstruction when using RBF instead of EBF for the OM kernel. The reconstructed surfaces only appear in two planes which is where the top and bottom whiskers on the lateral sides are located. The poor quality of the reconstruction motivates the use of the EBF kernel for mapping.