  This paper presents an assisted telemanipulation framework for reaching and
grasping desired objects from clutter. Specifically, the developed system
allows an operator to select an object from a cluttered heap and effortlessly
grasp it, with the system assisting in selecting the best grasp and guiding the
operator to reach it. To this end, we propose an object pose estimation scheme,
a dynamic grasp re-ranking strategy, and a reach-to-grasp hybrid force/position
trajectory guidance controller. We integrate them, along with our previous
SpectGRASP grasp planner, into a classical bilateral teleoperation system that
allows to control the robot using a haptic device while providing force
feedback to the operator. For a user-selected object, our system first
identifies the object in the heap and estimates its full six degrees of freedom
(DoF) pose. Then, SpectGRASP generates a set of ordered, collision-free grasps
for this object. Based on the current location of the robot gripper, the
proposed grasp re-ranking strategy dynamically updates the best grasp. In
assisted mode, the hybrid controller generates a zero force-torque path along
the reach-to-grasp trajectory while automatically controlling the orientation
of the robot. We conducted real-world experiments using a haptic device and a
7-DoF cobot with a 2-finger gripper to validate individual components of our
telemanipulation system and its overall functionality. Obtained results
demonstrate the effectiveness of our system in assisting humans to clear
cluttered scenes.
