Using Contacts During Robot Manipulation to Mapand Reconstruct a Scene
Joao Bimbo

Andrew S. Morgan and Aaron M. Dollar

Universidade Lusofona de Humanidades e Tecnologias
Lisboa, Portugal
Email: joao.bimbo@ulusofona.pt

Yale University
New Haven (CT) USA
Email: {andrew.morgan;aaron.dollar}@yale.edu

Abstract—Robot grasping and manipulation in real world
scenarios typically imply i) objects of unknown shape, ii) cluttered,unstructured settings with a high degree of uncertainty, andiii) contacts between the grasped object and obstacles in theenvironment. In this paper we present an approach to reconstruct
the shape of a grasped object and map the environment bymeasuring the contact forces while a robot tries to retrieve
an object from a scene. We use a probabilistic approach thatcomputes the probability of occupancy of two 3 dimensional maps
(one for the grasped object and one for the environment), where
the update on each map informs the construction of the other. Our
method relies only on the force and torque measurements fromthe robot end-effector to find the line of action of the interaction
forces, compute the possible contact locations and populate thetwo occupancy maps. Here we present our method, that we named
SMORE (Simultaneous Mapping and Object Reconstruction), anddiscuss some future developments of this approach.

I. INTRODUCTION
When robots operate outside laboratories and factories, inscenarios such as our homes or the outdoors, there is an
inherent uncertainty that comes with being in these unstructuredenvironments. This uncertainty brings about a number ofdifficulties and challenges when a robot has to grasp andmanipulate objects. In these tasks, there is typically limited apriori knowledge on object properties, such as its friction, mass,and even its shape, as well as the shape of the environment(map). Then, as the robot interacts with the world, there isa high chance of unexpected collisions between robot andenvironment or between the grasped object and the environment.
It is also important to note that in some scenarios the usage ofcomputer vision may be limited, particularly when there is asignificant amount of clutter and disarray. In these situations,other sensing modalities such as tactile and force sensing canbe useful to obtain information relevant to the task.
Here we detail a method that uses the force measurements
when a grasped object collides with obstacles in the environment while trying to retrieve it, to simultaneously reconstruct
the shape of the object and the obstacles in the environment.
This method populates two 3D maps with the probability thatthe voxels are occupied and, as we learn more about oneof the maps (object or environment), so it becomes easier toconstruct the other. We devised two probability laws to update
the maps: one is applied when a contact is detected, the possible
contact locations are computed and the voxels along thatline are updated; the second probability law is applied when(a) Discretization along the force line-of-action
L(b) Obstacles are detected after a number ofcontacts
Fig. 1. Simulated environment: S-shaped object and two vertical peg obstacles.
(a) Collisions between the object and an obstacle generate a force, and its lineof-action can be computed. (b) As multiple collisions occur along different
line-of-actions, we can probabilistically determine the occupancy probability.

there is no force detected, and so the object is not in contact
with an obstacle. We consider an application of our method,where an object of unknown, complex shape, is “tangled” withobstacles in the environment. The system repeatedly attempts
to retrieve the object by planning a collision-free path with
its current knowledge of the object and map geometries. As it
fails to retrieve the object and hits the obstacles, the maps areconstructed and refined until enough of the object and obstacles
is known to achieve a successful retrieval. This method waspreviously presented in [1], and here we discuss some of thelimitations and future possibilities of this work.
II. METHODS
The proposed method builds and populates two 3D maps:
Mw maps the environment and is attached to a fixed frame,and Mo moves with the robot end-effector describes theshape of the grasped object. The occupancy of each voxel
is updated sequentially and taking into account the current
probabilities. When a collision is detected, the occupancy
probabilities increase for the possible contact locations, while
the opposite occurs when the robot is moving in free space
without hitting an obstacle.

(a)

(b)

(c)

(d)

(e)

(f)

Fig. 2. Object untangling: method to map the environment and reconstruct the grasped object. Top row: Object reconstruction in green/red, mapped obstacles
in black/white, planned path in cyan/purple. Only voxels with a probability of occupancy higher than 50% are displayed. Bottom row: Simulation environment.

A. Probability Laws
1) Probability law for contact: When there is a collision
between the object and the environment, we first calculatethe line of action of that force given the measured force andtorque. We can then find the possible contact location as theset of voxels L ⊂ M that are intersected by this line. We
update the occupancy probability of occupancy of each voxel
P (Op ), p ∈ L with the probability law 1 :PP′ +P (Op )t+1 =Π1−
1 − PP′!
· PP′
(1)

1−Π

where′

P = P (Op ) , P ′ = P (¬Op )Π=Qn∈L (1 − P (On′) · P (On ))2) Probability Law for free space: When there is no contact
we can also use that information to update our maps. In
these situations there cannot be two coincident voxels thatare both occupied, since otherwise there would be a collision
and we would measure a force. So, the occupancy probability
is decreased for every pair of overlapping voxels according tothe law:
′P (Op )t+1 =P (Op ) · (1 − P (Op ))1 − P (Op ) · P (Op′ )(2)which states that the occupancy probability P (Op ) is obtained
simply by dividing the cases where p is occupied (and thusp′ is free) by the probability of no collision. The converse′
operation is also applied to p′ to obtain P (Op )t+1 .

1 for more details on this derivation see ref. [1]

I I I . R E S U LT S
An example is shown in Fig. 1 for the calculation of theforce line of action, and the environment map after a numberof collisions. The probability that a voxel is occupied is higher
at the intersection of these lines. We applied the method in
simulation to the problem of removing an object of unknown
geometry from a situation where it is tangled in an environment.
An RRT planner was used to find collision-free paths given
the current knowledge of object and map. Figure 2 shows anexecution of the task. At first (2a and 2b), the robot attempts togo in a straight line, after which it collides with the environment
several times. At the point shown in 2c, parts of the object
and the red peg in the environment have been reconstructed.
At point 2d and 2e the scene is almost fully reconstructed but
the removal is still unsuccessful until point 2f where the object
has been fully reconstructed and the planner is able to devise
a path that does not collide with the environment.
I V. D I S C U S S I O N
The proposed method works reasonably well, and outperformed a benchmark c-space planner and a reinforcement
learning approach in the previous task. It does suffer, however,from a number of limitations and could benefit from some
improvements. The main drawback is the inability to deal
with multiple simultaneous contacts. Secondly, the order atwhich the measurements arrive influences the result. Finally,
the method assumes the object is rigidly attached to the endeffector frame, which might not be the case if the forces
displace it within the robot hand. In terms of improvements, weare considering the combination of a vision system to provide
at least partial information, and to use optimal planners thatuse occupancy probability, rather than deciding that a state isvalid or invalid.
REFERENCES[1] Joao Bimbo, Andrew S. Morgan, and Aaron M. Dollar.
Force-based simultaneous mapping and object reconstruction for robotic manipulation. IEEE Robotics and Automation Letters, 7(2):4749–4756, 2022.