2012 IEEE International Conference on Robotics and Automation

RiverCentre, Saint Paul, Minnesota, USA

May 14-18, 2012

On-line Next Best Grasp Selection for In-Hand Object 3D Modeling

with Dual-Arm Coordination
Atsushi Tsuda, Yohei Kakiuchi, Shunichi Nozawa, Ryohei Ueda, Kei Okada and Masayuki Inaba
(The University of Tokyo)
Abstract Humanoid robots working in a household environment need 3D geometric shape models of objects for recognizing
and managing them properly. In this paper, we make humanoid
robots creating models by themselves with dual-arm re-grasping
(Fig.1 ). When robots create models by themselves, they should
know how and where they can grasp objects, how their hands
occlude object surfaces, and when they have seen every surface
on an object. In addition, to execute efficient observation with
less failure, it is important to reduce the number of re-grasping.
Of course when the shape of objects is unknown, it is difficult to
get a sequence of grasp positions which fulfills these conditions.
This determination problem of a sequence of grasp positions
can be expressed through a graph search problem. To solve
this graph, we propose a heuristic method for selecting the
next grasp position. This proposed method can be used for
creating object models when 3D shape information is updated
on-line. To evaluate it, we compare the result of the re-grasping
sequence from this method with the optimal sequence coming
out of breadth first search which use 3D shape information.
Also, we propose an observation system with dual-arm regrasping considering the points when humanoid robots execute
observation in the real world. Finally, we show the experiment
results of construction of 3D shape models in the real world
using the heuristic method and the observation system.

I. INTRODUCTION
Many researchers have tackled the issue of reconstructing
3D shape models of unknown objects. If humanoid robots
can construct 3D geometric models of unknown objects
by themselves, they can quite easily manage household
objects. When robots construct 3D shape models in the real
world, occlusion is a big problem. To solve the problem
of occlusion, we propose the system of construction of 3D
shape models by means of object observation by humanoid
robots with re-grasping.
With re-grasping, the problem of occlusion by hands and
the kinematics limitation can be solved more easily. The
problem of the selection of the next grasp positions can be
represented by a graph search problem. This graph nodes are
grasp position candidates and two nodes are connected when
the motion execution of re-grasping is possible. We call this
graph a dual-arm observation graph.
However, when shapes of objects are unknown, it is
difficult to find an optimal sequence of grasp positions
in which the number of re-grasping is minimum. So we
propose a heuristic method just using information of grasp
positions. This method can apply to a case when shape
information is updated on-line. To evaluate this method, we
M.Inaba, K.Okada, Y.Kakiuchi, S.Nozawa, R.Ueda and A.Tsuda are with
Mechano-Informatics The University of Tokyo, 7-3-1 Hongo, Bunkyo-ku,
Tokyo 113-8656, Japan tsuda@jsk.t.u-tokyo.ac.jp

978-1-4673-1405-3/12/$31.00 2012 IEEE

compute optimal solutions with given 3D geometric shape

information, and compare the simulation results using these
two methods.
In the real world, we have to consider the unexpected
movement of objects in hand measurement error and grasp
failure. We propose the following approach to solve these
problems.
Finally, we show the results of our model construction
experiments using the system mentioned below.
II. RELATED WORKS
There has been much research about view point selection
to get geometric information of objects. This is called the
Next Best View Problem. [1]. Reed et al. constructed a 3D
geometric model in simulation environment where a object
exists alone [2].
Also, many researchers worked on the acquirement of 3D
shape models in the real world. Their research has been categorized into two cases: move sensors or manipulate objects.
SLAM [3] is research about environment geometric shape
reconstruction by mobile robots with sensors. Engelhard et
al. constructed a modeling system that people move sensors
[4], and Montemerlo et al. took an approach in which robots
with a depth sensor move around environment[5].
As compared to the rapid progress in research about environment reconstruction, less research about modeling with
controlling objects has been done. Weise et al. constructed
a system in which people manipulate objects in front of a
camera [6], and Fox et al. reconstructed a 3D shape model
using a robot with a single arm that manipulated objects [7].
Re-grasping is one of the big issues in the reconstruction
with robot manipulation.
However, there are a few researchers focusing on the great
possibility that with dual-arm re-grasping observation, robots
can more easily construct non-occluded 3D shape model. If
robots with a single arm re-grasp an object, they have to
use the environment : putting an object down to table in
static way. To put an object down, the robot has to calculate
static putting position by considering the next grasp position
limited by the kinematics of the arm. So it is difficult to
get a non-occluded model of general shape objects, such as
curvature. If objects are apart from robot control, there is
high possibility of unexpected posture changes of objects.
Manipulation and planning with dual-arm coordination
were formulated since early times [8]. Recently, task planning in the real environment with dual-arm coordination has

1799

Fig. 1. Dual-arm observation concept. Robots acquire whole shape information with iterating observation and re-grasping. Selection of the next grasp
position is executed with the dual-arm observation graph which is updated on-line with 3D shape information.

been investigated. Dillman et al. executed simulation of the

re-grasping task of known shape objects [9].
III. OBSERVATION SYSTEM WITH DUAL-ARM
RE-GRASPING
In this section, we show the outline of our re-grasping
observation system. Fig.2 shows the system of construction
of 3D shape model with observation based on dual-arm regrasping, and Algorithm.1 describes the detail of this system
flow.

Fig. 2. Humanoid system of dual-arm observation.

First: Solving inverse kinematics to observe from each view point. When the
solution exists, robot takes the posture and gets objects shape information.
Second: Using observed 3D point cloud, check that observation of objects
is finished.
Third: When the observation has not finished yet, select next grasp position
using dual-arm observation graph.
Fourth: execute re-grasping, and go back to first.

Algorithm 1 Algorithm of observation based on dual-arm

re-grasping
1: i 0 : the number of re-grasping
2: V : given view points
3: P : all of observed 3D point cloud
4: Pi : all of observed 3D point cloud in i-th grasp
5: Pi,j : observed 3D point cloud from a view point vj in
i-th grasp
6: IsF inishObservation = f alse
7: while !IsF inishObservation do
8:
for all vj V do
9:
if exists IK solution to observe from vj then
10:
move robot
11:
get 3D point cloud on object Pi,j
12:
append Pi,j to Pi
13:
end if
14:
end for
15:
Voxel Filtering to P , P i
16:
if checkf inishf n(P , Pi ) == true then
17:
IsF inishObservation true
18:
else
19:
re-grasping
20:
append Pi to P
21:
ii+1
22:
end if
23: end while

observe from each view point. When the solution exists, the
robot takes the posture and gets objects shape information.
B. Check Finish Observation
Using the observed 3D point cloud, robots check that the
observation of objects is finished. We mention the detail of
the functions for check finish observation later.

A. Observe Object

C. Compute/Select Grasp Position

Robots observe an object from the view points fixed to the

objects coordinates. These view points are centers of faces of
a geodesic dome represented by regular icosahedron (the left
figure of Fig.3 ). We solve inverse kinematics that robots can

When the observation has not finished yet, compute grasp

position and select the next grasp position. We use OpenRAVE for grasp position computation[10]. The target model
for grasp computation is convex hull of 3D point cloud

1800

grasp position
which arm, left or right arm is used
A node connects to the others in the case mentioned below.
the two grasps have different arms
re-grasping is possible
a solution of inverse kinematics enabled the relative
transformation between dual-arm exists
no collision is detected when a humanoid robot take
the re-grasping posture

Fig. 3. Left: Given view points fixed to an object.

Positions of these view points are centers of faces of a geodesic dome
represented by icosahedron.
Right: Grasp position example.
Grasp positions are expressed in object coordinates.
When re-grasping, we solve inverse kinematics of which the input is the
transformation of two grasp positions.

A. Heuristic Search Just Using Information of Grasp Position

Fig.5 shows an overview of the heuristic search method
of dual-arm observation graph, and Algorithm.2 is the detail
of the method.

observed by then. If no next grasps is avaiable, robots give

up observation. We use the information of grasp positions,
which is shown in the right figure of Fig.3 . Grasp positions
are represented in object coordinates. We show this selection
method in section IV.
D. Re-grasp Object
Now that robots have selected the next grasp position,
execute re-grasping. In the real world, robots may fail to
grasp. So robots detect grasp failure and if failure is detected,
return to III-C.
IV. DUAL-ARM OBSERVATION GRAPH
It is possible to express this motion planning of a sequence
of re-grasping position through the graph search problem. We
call this graph dual-arm observation graph. Fig.4 shows an
overview of the dual-arm observation graph.

Fig. 5. Proposed heuristic method uses just grasp position information

which can be computed from 3D shape information updated on-line.
Black lines are the next grasp candidates, and red lines are the selected
paths. A observation finishes when the update ratio of 3D point cloud fall
below threshold.

Humanoid robots select next grasp position which is most

dissimilar one from previous grasp positions.
If grasps which do not collide with previous grasps
exist, select one randomly.
If there is no grasp which does not collide with previous
grasps, select one whose minimum rotation angle of
relative coordinates between current grasp position and
colliding previous grasp position is the biggest among
the candidates.
Algorithm.3 describes how to check observation finish.
Humanoid robots check the update ratio of the number of
observed 3D point cloud whose density is constant.
Fig. 4. Dual-arm observation graph concept. Nodes have information of
grasp positions and which arm (left or right arm) is used. Two nodes are
connected when the motion execution of re-grasping is possible. We can
determine the grasp sequence using this graph.

Every node includes information which we enumerate

below:

B. Breadth First Search Using Shape Information of Objects

We solve through breadth first search of the dual-arm
observation graph, with shape information of objects. When
shape information is given, we can get an optimal solution of the dual-arm observation graph Fig.6 . We append
shape information of objects represented with 3D point

1801

Algorithm 2 Algorithm of selection from next grasp position

1: G : next grasp position candidates
2: Gp,s : previous grasp positions with same hand
3: Gp,a : previous grasp positions with another hand
4: Gnc : grasps no collision with previous grasp
5: Gcps : grasps collide with previous same hand grasp
6: Gcpa : grasps collide with previous another hand grasp
7: for g G do
8:
for gp,s Gp,s do
9:
if g collide gp,s then
10:
push g to Gcps with difference of rotation
11:
end if
12:
end for
13:
for gp,a Gp,a do
14:
if g collide gp,a then
15:
push g to Gcpa
16:
end if
17:
end for
18: end for
19: sort Gcps with difference of rotation
20: append (Gnc , Gcpa , Gcps )

Fig. 6. Breadth first search for computation the minimum number of regrasping. The number can be computed when the shape information is given.
Black lines are the next grasp candidates, and red lines are the selected paths.
TABLE I
T HE NUMBER OF RE - GRASPING AND THE ACQUIRED 3D

Algorithm 3 checkf inishf n(P , Pi ) in proposed heuristic

search
1: r : update
( ratio threshold )
len(Pi )
2: return len(P )+len(P
<r
i)

breadth

cloud Pi which can get when robots grasp the position of

each node. We use the function to check observation finish checkf inishf n(P , Pi ) described in Algorithm.4 This
method can only apply the case that the shape information
of objects is given.
Algorithm 4 checkf inishf n(P , Pi ) in breadth first search
1: Pobj : 3D point cloud represent given shape model
2: thr : distance threshold to check the point observed
3: for all Pobj,i Pobj do
4:
if distance (Pobj,i , N earestN eighbor (Pobj,i , P )) <
thr then
5:
return(f alse)
6:
end if
7: end for
8: return(true)
Compute the 3D point cloud Pobj which represents the
whole shape of them with given shape information of objects.
Make the density of both Pobj and 3D point cloud P which
can be observed in the grasp sequence constant. For each
point pobj,i ( Pobj ), search the nearest point Pi ( P ). For
all i( [0, len(Pobj )], the distance between Pobj,i and Pi is
smaller than thr, then robots finish observation.
In Section V, we show the comparison of the result
simulated with these two methods.

method
heuristic

breadth

SHAPE RATIO

method
heuristic

1.0

2.5

96.3%

1.11

2.2

100.0%

1.0

2.0

100.0%

1.71

2.0

99.1%

1.0

2.0

100.0%

1.28

2.0

100.0%

1.26

2.8

100.0%

1.4

2.4

98.3%

1.09

2.6

94.8%

V. COMPARISON BETWEEN TWO METHODS

Table.I shows the results of the simulation solving dualarm observation graph. This table is composed of the average
number of re-graspings (we start computation from 5 to 7
grasp positions) got by two methods, and the average ratio
of 3D shape information which can be observed in heuristic
method. For example, Dual-arm observation graph of mug
cup has 79 nodes and 132 edges.
The average of the number of re-grasping times with the
optimal solution is 1.11, with the heuristic solution is 2.77.
In the experiments of nine objects, the average number of
re-grasping with heuristic method was 1.66 more than that
of the optimal solutions. We got this result because of nonconsideration of the viewing of objects. We can easily notice
the first re-grasping position is different from these two
solutions. In this paper, our experiments used grasp position
only when selecting next grasp position, but in the future
we may also have to evaluate the relation between grasp
position, inverse kinematics solution existence and viewing
to get the optimal solution with heuristic method.

1802

Using our method robots can observe most of all shape

information of objects. We emphasize on the point that the
method can apply in case of objects observation in which
shape information is updated on-line. This heuristic method
yields 98.7% of observation on the whole on an average.
VI. DUAL-ARM OBSERVATION SYSTEM IN THE
REAL WORLD
In the real world, robots can not execute the dual-arm
observation as simulated.
There are some big differences between simulation and
the real world.
there is measurement error of 3D point cloud data, and
objects are not fixed to robots(See VI-A).
there is a possibility of failure of re-grasping(See VI-B).
We mention approaches to solve these problems.
We explain the color ICP algorithm and the method to
detect grasp failure in detail.

Fig. 7. Example of grasp fail detection

left : HRP2JSKNT tried to grasp the thin position of object, and failed to
grasp.
center : HRP2JSKNT gave up grasping this position.
right : selected another grasp position and execute re-grasping motion, and
succeeded at grasp.

Fig. 8. Left : Color range sensor mounted on HRP2JSKNT

Right : 6 DOF three fingers left hand

A. Point Cloud Registration with Color ICP Algorithm

3D point cloud which depth sensors measure have measurement error, and angle-measurement potentiometers installed in robot joints also have errors, so we can not know
the true value of these data. To correct these errors, we use
the Color ICP algorithm [11], [12].
[
dist(Pi , Qj ) = (xi xj )2 + (yi yj )2 + (zi zj )2
{
}] 1
+ (ri rj )2 + (gi gj )2 + (bi bj )2 2 (1)

robots should detect the failure of grasp, and reduce the risk
to drop objects.
1) Send a command to joint angles making the robot
fingers and thumb contact when there is no obstacle
between them. From the difference of the command
and the measurement value of potentiometer, robots
detect grasp failure.
2) If the difference is big, robots judge the re-grasping
Qnn(Pi ) = N earestN eighbor(Pi )
(2)
success, and shift to observation.

3)
Or,
robots give up grasping the grasp position, seeval = (dist(Pi , Qnn(Pi ) ))
(3)
lect
another grasp position, and attempt another re(i [0, n = size(P ) | dist(Pi , Qnn(Pi ) ) < threshold])
grasping.
For example, we intend to do the registration of two point
Fig.7 shows the example of detection of grasp failure.
clouds, P and Q. The positions of these point clouds are
initialized with measurement information of joint angles so
VII. EXPERIMENT OF CONSTRUCTING SHAPE
that the points are aligned correctly if there is no error.
MODEL WITH DUAL-ARM RE-GRASPING
Pi (= (xi yi zi ri gi bi )) and Qj (= (xj yj zj rj gj bj )) are
In this section, we show the result of experiments of
points in these point cloud. These points have information
constructing 3D shape models with dual-arm re-grasping,
of 3D (XYZ) position and 3D (RGB) color. For all Pi (i
using the system and heuristic method we mentioned above.
[0, n]), search the point Qnn(Pi ) which weighted 6D distance
We use HRP2JSKNT, a humanoid robot HRP-2 that we
(calculated from (1), is a coefficient of RGB values) is
extended [14] [15]. HRP2JSKNT has three 6DOF fingers
the smallest in Q (2). The evaluation function is the sum
and a color range sensor set which is composed of a range
of these distances that are smaller than threshold (4). We
sensor and stereo camera[12]. Fig.8 shows this equipment.
set a point cloud observed from one view point to P , and
We set these preconditions.
the other observed point clouds to Q, compute minimum
Humanoid robot can grasp any position of objects.
transformation, set P for all point clouds and iterate until
Objects have no joints and are adequately rigid.
convergence. Using this evaluation function and downhill
When robots look at a center of bounding box of an
simplex method [13], compute the transformation in which
object, the edges of objects are not out of field of view.
the evaluation value is minimum. This transformation is also
We solve inverse kinematics such that the distance
used to update the position of observing an object because
between color range sensor and objects is larger than
there is a risk that objects slightly move in robot hand.
500[mm]. The minimum angle of view of color range
B. Grasp Fail Detection
sensor is 34.6[deg], so we use objects in which the
Because computation of grasp position is not perfect,
length of diagonal line of bounding box is smaller than
robots may fail to grasp and drop down objects. Therefore,
300[mm] < 311[mm] < 500[mm]tan(34.6[deg])2.
1803

The observation starts after we put objects into the robots

hand, so we do not consider the first grasp computation.
Fig.9 shows the motion snapshots and created models by
a humanoid robot.

Also, we proposed some method to solve the problem

particular in practice, and constructed a system of dual-arm
observation in the real world. We showed the experiment
results of construction of a 3D shape model using this algorithm and system, and our approach proved to be valid. In
future work, we aim to minimize the number of re-graspings
by taking viewing and occlusion that arise from hand and
kinematics into account. Also, we work on task planning
including recognition and manipulation using acquired 3D
shape models.
R EFERENCES

Fig. 9.

objects and constructed models

There are 2 grasp failures in experiments of upper 2

objects, but the humanoid robot did not drop objects, and the
number of re-graspings is 3 (row 1), 4 (row 2), 4 (row 3)
and 3 (row 4). It is difficult to evaluate whether complicated
shape is reconstructed, so we evaluate 3D shape model of
box shape object which can get a true model easily (row
3 of Fig.9 ). Differences between the length of a side of
bounding box of acquired 3D point cloud models and actual
measurement is within 10[mm]. The standard deviation
of distance between points and the nearest face of a true
model is 3.15[mm]. Also we calculated how much shape
information is gotten with same method Algorithm.4 , and we
got 100.0% shape information. So we can get non-occluded
model for box shape object.
VIII. CONCLUSION
In this paper, we proposed a humanoid robot system to acquire non-occluded 3D shape models with observing objects
with dual-arm coordination, and we showed how to determine next grasp position and the determination problem of
the sequence of grasp positions could be expressed through
a graph (dual-arm observation graph) search problem. To
reduce the number of re-graspings in constructing models,
we proposed a heuristic method just using information of
grasp position. To evaluate this method, we computed the
optimal solutions with shape information, and compared
the two methods. We showed robots can not get optimal
observation with the heuristic method, but they can execute
observation of the shape information on the whole one extra
re-grasping than optimal observation on an average.

[1] William R. Scott, Gerhard Roth, and Jean-Francois Rivest. View

planning for automated three-dimensional object reconstruction and
inspection. ACM Comput. Surv., Vol. 35, pp. 6496, March 2003.
[2] Michael K. Reed, Peter K. Allen, and Ioannis Stamos. Automated
model acquisition from range images with view planning. In Proceedings of the 1997 Conference on Computer Vision and Pattern
Recognition (CVPR 97), CVPR 97, pp. 7277, Washington, DC,
USA, 1997. IEEE Computer Society.
[3] M. W. M. Gamini Dissanayake, Paul Newman, Steven Clark, Hugh F.
Durrant-whyte, and M. Csorba. A solution to the simultaneous
localization and map building (slam) problem. IEEE Transactions
on Robotics and Automation, Vol. 17, pp. 229241, 2001.
[4] Nikolas Engelhard, Felix Endres, Jurgen Hess, Jurgen Sturm, and
Wolfram Burgard. Real-time 3d visual slam with a hand-held rgbd camera. In Proc. of the RGB-D Workshop on 3D Perception in
Robotics at the European Robotics Forum, Vasteras, Sweden, April
2011.
[5] Michael Montemerlo, Sebastian Thrun, Daphne Koller, and Ben Wegbreit. Fastslam: A factored solution to the simultaneous localization
and mapping problem. In In Proceedings of the AAAI National
Conference on Artificial Intelligence, pp. 593598. AAAI, 2002.
[6] T. Weise, T. Wismer, B. Leibe, and L. Van Gool. In-hand scanning
with online loop closure. In IEEE International Workshop on 3-D
Digital Imaging and Modeling (ICCV), October 2009.
[7] Michael Krainin, Brian Curless, and Dieter Fox. Autonomous generation of complete 3d object models using next best view manipulation
planning. In Proceedings of The 2011 IEEE International Conference
on Robotics and Automation, pp. 5031 5037, 5 2011.
[8] Yoshihito Koga and Jean-Claude Latombe. On multi-arm manipulation
planning. In Proc. IEEE Int. Conf. on Robotics and Automation, pp.
945952, 1994.
[9] Nikolaus Vahrenkamp, Dmitry Berenson, Tamim Asfour, James
Kuffner, and Rudiger Dillmann. Humanoid motion planning for dualarm manipulation and re-grasping tasks. In IEEE/RSJ International
Conference on Intelligent Robots and Systems (IROS 09), October
2009.
[10] Rosen Diankov. Automated Construction of Robotic Manipulation
Programs. PhD thesis, Carnegie Mellon University, Robotics Institute,
August 2010.
[11] Andrew Johnson and Sing Bing Kang. Registration and Integration
of Textured 3-D Data. In Proceedings of InternationalConference on
Recent Advances in 3-D Digital Imaging and Modeling (3DIM 97),
pp. 234 241, 1997.
[12] Yohei Kakiuchi, Ryohei Ueda, Kei Okada, and Masayuki Inaba.
Creating household environment map for environment manipulation
using color range sensors on environment and robot. In Proceedings of
The 2011 IEEE International Conference on Robotics and Automation,
pp. 305310, 5 2011.
[13] J. A. Nelder and R. Mead. A simplex method for function minimization. Computer Journal, Vol. 7, pp. 308313, 1965.
[14] H.Hirukawa, F.Kanehiro, K.Kaneko, S.Kajita, K.Fujiwara, Y.Kawai,
F.Tomita, S.Hirai, K.Tanie, T.Isozumi, K.Akechi, T.Kawasaki, S.Ota,
K.Yokoyama, H.Honda, Y.Fukase, J.Maeda, Y.Nakamura, S.Tachi,
and H.Inoue. Humanoid Robotics Platforms developed in HRP.
In Proceedings of the 2003 IEEE-RAS International Conference on
Humanoid Robots (Humanoids 2003), 2003.
[15] K.Kaneko, F.Kanehiro, S.Kajita, H.Hirukawa, T.Kawasaki, M.Hirata,
K.Akachi, and T.Isozumi. Humanoid Robot HRP-2. In Proc. Int. Conf.
on Robotics and Automation(ICRA), pp. 10831090, 2004.

1804