This paper addresses the challenge of autonomous excavation of challenging terrains, in particular those that are prone to jamming and inter-particle adhesion when tackled by a standard penetrate-drag-scoop motion pattern. Inspired by human excavation strategies, our approach incorporates oscillatory rotation elements - including swivel, twist, and dive motions - to break up compacted, tangled grains and reduce jamming. We also present an adaptive impedance control method, the Reactive Attractor Impedance Controller (RAIC), that adapts a motion trajectory to unexpected forces during loading in a manner that tracks a trajectory closely when loads are low, but avoids excessive loads when significant resistance is met. Our method is evaluated on four terrains using a robotic arm, demonstrating improved excavation performance across multiple metrics, including volume scooped, protective stop rate, and trajectory completion percentage.

High-resolution visuotactile sensors provide detailed contact information that is promising to infer the physical properties of objects in contact. This paper introduces a novel technique for high-resolution stiffness estimation of heterogeneous deformable objects using the Punyo bubble sensor. We developed an observation model for dense contact forces to estimate object stiffness using a visuotactile sensor and a dense force estimator. Additionally, we propose a neural Volumetric Stiffness Field (VSF) formulation that represents stiffness as a continuous function, which allows dynamic point sampling at visuotactile sensor observation resolution. The neural VSF significantly reduces artifacts commonly associated with traditional point-based methods, particularly in stiff inclusion estimation and heterogeneous stiffness estimation. We further apply our method in a blind localization task, where objects within opaque bags are accurately modeled and localized, demonstrating the superior performance of neural VSF compared to existing techniques. Project page: https://hjh371.github.io/Neural-VSF/.

Fruit monitoring plays an important role in crop management, and rising global fruit consumption combined with labor shortages necessitates automated monitoring with robots. However, occlusions from plant foliage often hinder accurate shape and pose estimation. Therefore, we propose an active fruit shape and pose estimation method that physically manipulates occluding leaves to reveal hidden fruits. This paper introduces a framework that plans robot actions to maximize visibility and minimize leaf damage. We developed a novel scene-consistent shape completion technique to improve fruit estimation under heavy occlusion and utilize a perception-driven deformation graph model to predict leaf deformation during planning. Experiments on artificial and real sweet pepper plants demonstrate that our method enables robots to safely move leaves aside, exposing fruits for accurate shape and pose estimation, outperforming baseline methods. Project page: https://shaoxiongyao.github.io/lmap-ssc/.

Metric-semantic 3D mapping is the process of creating class-labeled 3D maps by fusing the information from images captured by a moving camera. The memory usage required by standard solutions grows linearly with the number of semantic classes being considered, which can pose a bottleneck in large and many-class scenes. This paper proposes two novel methods for compressing the memory used by semantic fusion: calibrated top-k histogram and encoded fusion. The first method maintains, for each voxel, only the counts of the k most likely classes, while the second method uses a neural network to encode all-class probability vectors into a k-dimensional latent space in which per-voxel fusion is performed. The fused result is then decoded, at query time, using another neural network. Experiments show that both methods preserve map accuracy and calibration even at low values of k, and per-voxel memory usage is linear in k. The proposed methods can achieve real-time semantic fusion with 150 classes on commodity GPUs in building-scale scenes where prior approaches run out of memory.

Contact-implicit trajectory optimization (CITO) is an effective method to plan complex trajectories for various contact-rich systems including manipulation and locomotion. CITO formulates a mathematical program with complementarity constraints (MPCC) that enforces that contact forces must be zero when points are not in contact. However, MPCC solve times increase steeply with the number of allowable points of contact, which limits CITO's applicability to problems in which only a few, simple geometries are allowed us to make contact. This article introduces simultaneous trajectory optimization and contact selection (STOCS), as an extension of CITO that overcomes this limitation. The innovation of STOCS is to identify salient contact points and times inside the iterative trajectory optimization process. This effectively reduces the number of variables and constraints in each MPCC invocation. The STOCS framework, instantiated with key contact identification subroutines, renders the optimization of manipulation trajectories computationally tractable even for high-fidelity geometries consisting of tens of thousands of vertices.

Semantic 3D mapping, the process of fusing depth and image segmentation information between multiple views to build 3D maps annotated with object classes in real-time, is a recent topic of interest. This paper highlights the fusion overconfidence problem, in which conventional mapping methods assign high confidence to the entire map even when they are incorrect, leading to miscalibrated outputs. Several methods to improve uncertainty calibration at different stages in the fusion pipeline are presented and compared on the ScanNet dataset. We show that the most widely used Bayesian fusion strategy is among the worst calibrated, and propose a learned pipeline that combines fusion and calibration, GLFS, which achieves simultaneously higher accuracy and 3D map calibration while retaining real-time capability and adding only 525 learned parameters to the pipeline. We further illustrate the importance of map calibration on a downstream task by showing that incorporating proper semantic fusion to an indoor object search agent improves its success rates.

Teleoperated avatar robots allow people to transport their manipulation skills to environments that may be difficult or dangerous to work in. Current systems are able to give operators direct control of many components of the robot to immerse them in the remote environment, but operators still struggle to complete tasks as competently as they could in person. We present a framework for incorporating open-world shared control into avatar robots to combine the benefits of direct and shared control. This framework preserves the fluency of our avatar interface by minimizing obstructions to the operator’s view and using the same interface for direct, shared, and fully autonomous control. In a human subjects study (N=19), we find that operators using this framework complete a range of tasks significantly more quickly and reliably than those that do not.

Soft-bubble tactile sensors have the potential to capture dense contact and force information across a large contact surface. However, it is difficult to extract contact forces directly from observing the bubble surface because local contacts change the global surface shape significantly due to membrane mechanics and air pressure. This paper presents a model-based method of reconstructing dense contact forces from the bubble sensor’s internal RGBD camera and air pressure sensor. We present a finite element model of the force response of the bubble sensor that uses a linear plane stress approximation that only requires calibrating 3 variables. Our method is shown to reconstruct normal and shear forces significantly more accurately than the state-of-the-art, with comparable accuracy for detecting the contact patch, and with very little calibration data.

This paper presents a novel experience-based technique, called EHGO, for sample-efficient adaptive control of nonlinear systems in the presence of dynamical modeling errors. The starting point for EHGO is a database seeded with many trajectories optimized under a reference estimate of real system dynamics. When executed on the real system, these trajectories will be suboptimal due to errors in the reference dynamics. The approach then leverages a hybrid gradient optimization technique, GRILC, which observes executed trajectories and computes gradients from the reference model to refine the control policy without requiring an explicit model of the real system. In past work, GRILC was applied in a restrictive setting in which a robot executes multiple rollouts from identical start states. In this paper, we show how to leverage a database to enable GRILC to operate across a wide envelope of possible start states in different iterations. The database is used to balance between start state proximity and recentness-of-experience via a learned distance metric to generate good initial guesses. Experiments on three dynamical systems (pendulum, car, drone) show that the proposed approach adapts quickly to online experience even when the reference model has significant errors. In these examples EHGO generates near-optimal solutions within hundreds of epochs of real execution, which can be orders of magnitude more sample efficient than reinforcement learning techniques.

This paper introduces a method for learning the force response of heterogeneous, deformable objects directly from robot sensor data without prior knowledge. The method estimates an object's force response given robot force or torque measurements using a novel volumetric stiffness field representation and point-based contact simulator. The stiffness of each point colliding with the robot is estimated independently and is updated upon each observed measurement using a projected diagonal Kalman filter. Experiments show that this method can update a stiffness field over 105points at 23 Hz or higher, and is more accurate than learning-based methods in predicting torque response while touching artificial plants. The method can also be augmented with visual information to help extrapolate stiffness fields to distant parts of the touched object using only a small number of touches.

Robot manipulation of multiple objects is an important topic for applications including warehouse automation, service robots performing cleaning, and large-scale object sorting. Although problems can range in complexity from a few objects to large disordered piles, autonomy remains a significant technical challenge due to the high-dimensional joint configuration space of the robot and all objects, the complex dynamics of object interaction, and the ambiguity and occlusion caused by clutter. This article surveys a broad range of classical and state-of-the-art research in multiobject manipulation and categorizes them along the dimensions of tasks, perception, predictive models, and decision-making algorithms. It also covers emerging trends and open problems faced in the ongoing effort to realize robust multiobject manipulation systems in practice.

This paper presents a novel iterative closest points (ICP) variant, non-penetration iterative closest points (NPICP), which prevents interpenetration in 6DOF pose optimization and/or joint optimization of multiple object poses. This capability is particularly advantageous in cluttered scenarios, where there are many interactions between objects that constrain the space of valid poses. We use a semi-infinite programming approach to handle non-penetration constraints between complex, non-convex 3D geometries. NPICP is applied to a common use case for ICP as a post-processing method to improve the pose estimation accuracy of a rough guess. The results show that NPICP outperforms ICP, assists in outlier detection, and also outperforms the best result on the IC-BIN dataset in the Benchmark for 6D Object Pose Estimation.

Robots have to take highly responsive real-time actions, driven by complex decisions involving a pipeline of sensing, perception, planning, and reaction tasks. These tasks must be scheduled on resource-constrained devices such that the performance goals and the requirements of the application are met. This is a difficult problem that requires handling multiple scheduling dimensions, and variations in computational resource usage and availability. In practice, system designers manually tune parameters for their specific hardware and application, which results in poor generalization and increases the development burden. In this work, we highlight the emerging need for scheduling CPU resources at runtime in robot systems. We use robot navigation as a case-study to understand the key scheduling requirements for such systems. Armed with this understanding, we develop a CPU scheduling framework, Catan, that dynamically schedules compute resources across different components of an app so as to meet the specified application requirements. Through experiments with a prototype implemented on ROS, we show the impact of system scheduling on meeting the application's performance goals, and how Catan dynamically adapts to runtime variations.

Disinfection robots have applications in promoting public health and reducing hospital acquired infections and have drawn considerable interest due to the COVID-19 pan-demic. To disinfect a room quickly, motion planning can be used to plan robot disinfection trajectories on a reconstructed 3D map of the room's surfaces. However, existing approaches discard semantic information of the room and, thus, take a long time to perform thorough disinfection. Human cleaners, on the other hand, disinfect rooms more efficiently by prioritizing the cleaning of high-touch surfaces. To address this gap, we present a novel GPU-based volumetric semantic TSDF (Truncated Signed Distance Function) integration system for semantic 3D reconstruction. Our system produces 3D reconstructions that distinguish high-touch surfaces from non-high-touch surfaces at approximately 50 frames per second on a consumer-grade GPU, which is approximately 5 times faster than existing CPU-based TSDF semantic reconstruction methods. In addition, we extend a UV disinfection motion planning algorithm to incorporate semantic awareness for optimizing coverage of disinfection tra-jectories. Experiments show that our semantic-aware planning outperforms geometry-only planning by disinfecting up to 20% more high-touch surfaces under the same time budget. Further, the real-time nature of our semantic reconstruction pipeline enables future work on simultaneous disinfection and mapping. Code is available at: https://github.com/uiuc-iml/RA-SLAM

Recent progress in the field of robotic manipulation has generated interest in fully automatic object packing in warehouses. This article presents a formulation of the robotic packing problem that ensures stability of the object pile during packing and the feasibility of the robot motion while maximizing packing density. A constructive packing algorithm is proposed to address this problem by searching over the space of item positions and orientations. Moreover, a new heightmap minimization heuristic is shown to outperform existing heuristics in the literature in the presence of nonconvex objects. Two strategies for improving the robustness of executed packing plans are also proposed: 1) conservative planning ensures plan feasibility under uncertainty in model parameters; and 2) closed-loop packing uses vision sensors to measure placement errors and replans to correct for them. The proposed planner and error mitigation strategies are evaluated in simulation and on a state-of-the-art physical packing testbed. Experiments demonstrate that the proposed planner generates high-quality packing plans, and the error mitigation strategies improve success rates beyond an open-loop baseline from 83% to 100% on five-item orders.

This paper proposes a hybrid planning framework that generates complex dynamic motion plans for jumping legged robots to traverse challenging terrains. By employing a motion primitive, the original problem is decoupled as path planning followed by a trajectory optimization (TO) module that handles dynamics. A variant of a kinodynamic Rapidly-exploring Random Trees (RRT) planner finds a path as a parabola sequence between stance phases. To make this fast, a reachability informed control sampling scheme leverages a precomputed velocity reachability map. The path is post-processed to eliminate redundant jumps and passed to the TO module to find a dynamically feasible trajectory. Simulation results are presented where the proposed hybrid planner solves challenging terrains by executing multiple consecutive jumps, producing novel strategies to leap over large gaps by leveraging dynamics. In a physical experiment, the hybrid planner is tested on a real robot successfully traversing a challenging terrain.

Model predictive control (MPC) is a powerful feedback technique that is often used in data-driven robotics. The performance of data-driven MPC depends on the accuracy of the model, which often requires careful tuning. Furthermore, specifying the task with an objective function and synthesizing a feedback policy are not straightforward and typically lead to suboptimal solutions driven by trial and error. To address these challenges, we present a method to jointly optimize the data-driven system identification, task specification, and control synthesis of unknown dynamical systems. We use our method to develop AutoMPC3, a software package designed to automate and optimize data-driven MPC. Empirical evaluation on the pendulum swing-up, cart-pole swing-up, and half-cheetah running demonstrates that our method finds data-driven control policies that outperform offline reinforcement learning, without any hand-tuning.

Robot design is a time-consuming process involving repeated experiments in a variety of environments to optimize multiple, possibly conflicting performance metrics. Moreover, the optimal robot performance for a given design depends on how the robot adapts its behavior to its environment. We propose a multi-objective Bilevel Bayesian optimization (MO-BBO) technique to automate the process of form-behavior co-design. The approach expands the Pareto front of multiple metrics by simultaneously exploring the robot design and behavior. MO-BBO uses a bilevel optimization of the acquisition function with design and behavior parameters being the high- and low-level decision variables, respectively. In the low-level, we always choose environment-aware behaviors that maximize each metric. We evaluate MO-BBO in applications to grasping gripper design and bimanual arm placement, and show that our method can efficiently focus samples on the Pareto front and generate a diversity of designs.

UV radiation has been used as a disinfection strategy to deactivate a wide range of pathogens, but existing irradiation strategies do not ensure sufficient exposure of all environmental surfaces and/or require long disinfection times. We present a near-optimal coverage planner for mobile UV disinfection robots. The formulation optimizes the irradiation time efficiency, while ensuring that a sufficient dosage of radiation is received by each surface. The trajectory and dosage plan are optimized taking collision and light occlusion constraints into account. We propose a two-stage scheme to approximate the solution of the induced NP-hard optimization, and, for efficiency, perform key irradiance and occlusion calculations on a GPU. Empirical results show that our technique achieves more coverage for the same exposure time as strategies for existing UV robots, can be used to compare UV robot designs, and produces near-optimal plans.

We present a planner for large-scale (un)labeled object sorting tasks, which uses two types of manipulation actions: overhead grasping and planar pushing. The grasping action offers completeness guarantee under mild assumptions, and the planar pushing is an acceleration strategy that moves multiple objects at once. We make two main contributions: (1) We propose a bilevel planning algorithm. Our high-level planner makes efficient, near-optimal choices between pushing and grasping actions based on a cost model. Our low-level planner computes one-step greedy pushing or grasping actions. (2) We propose a novel low-level push planner that can find one-step greedy pushing actions in a semi-discrete search space. The structure of the search space allows us to efficiently make decisions. We show that, for sorting up to 200 objects, our planner can find near-optimal actions within 10 seconds of computation on a desktop PC.

We present non-convex maximal dissipation principle (NMDP), a time integration scheme for articulated bodies with simultaneous contacts. Our scheme resolves contact forces via the maximal dissipation principle (MDP). Whereas prior MDP solvers assume linearized dynamics and integrate using the forward multistep scheme, we consider the coupled system of nonlinear Newton-Euler dynamics and MDP and integrate using the backward integration scheme. We show that the coupled system of equations can be solved efficiently using a novel projected gradient method with guaranteed convergence. We evaluate our method by predicting several locomotion trajectories for a quadruped robot. The results show that our NMDP scheme has several desirable properties including: (1) generalization to novel contact models; (2) stability under large timestep sizes; (3) consistent trajectory generation under varying timestep sizes.

This paper presents a novel computational model to address the problem that contact is an infinite phenomena involving continuous regions of interaction. The problem is cast as a semi-infinite program with complementarity constraints (SIPCC). Rather than pre-discretize contacting surfaces into a finite number of contact points, we use semi-infinite programming (SIP) techniques that operate on the underlying continuous geometry, but dynamically determine a finite number of constraints that are most relevant to solving the problem. Then we solve the series of problems whose solutions converge toward one that contains a true optimum of the original SIPCC. We apply the model to a grasping pose optimization problem for a gripper and a humanoid robot, and our model enables the robots to find a feasible pose to hold (non-)convex objects while ensuring force and torque balance.

We present a bilevel, contact-implicit trajectory optimization (TO) formulation that searches for robot trajectories with learned soft contact models. On the lower-level, contact forces are solved via a quadratic program (QP) with the maximum dissipation principle (MDP), based on which the dynamics constraints are formulated in the upper-level TO problem that uses direct transcription. Our method uses a contact model for granular media that is learned from physical experiments, but is general to any contact model that is stick-slip, convex, and smooth. We employ a primal interior-point method with a pre-specified duality gap to solve the lower-level problem, which provides robust gradient information to the upper-level problem. We evaluate our method by optimizing locomotion trajectories of a quadruped robot on various granular terrains offline, and show that we can obtain long-horizon walking gaits of high qualities.

This article formulates two variants of packing problems in which the set of items is known, but the arrival order is unknown. The goal is to certify that the items can be packed in a given container and/or to optimize the size or cost of a container so that that the items are guaranteed to be packable, regardless of arrival order. The nondeterministically ordered packing (NDOP) variant asks to generate a certificate that a packing plan exists for every ordering of items. Quasi-online packing (QOP) asks to generate a partially observable packing policy that chooses the location of each item as their arrival order is revealed one-by-one, with all of the remaining items certified to be packable regardless of their future arrival order. Theoretical analysis demonstrates that even the simple subproblem of verifying the feasibility of a packing policy is NP-complete. Despite this worst case complexity, practical solvers for both NDOP and QOP are developed. Multiple extensions to the basic nondeterministic problem are presented, including packing with a fixed-capacity buffer and packing with equivalent objects. Experiments demonstrate that these algorithms can be applied to packing irregular 3-D shapes with stability and manipulator loading constraints.Note to Practitioners—Automatic packing of objects into containers, such as a shipping box, has applications in warehouse automation for e-commerce applications and less-than-truckload shipping. In many scenarios, a container needs to be preselected for a set of objects before they arrive, and the order of the objects cannot be controlled. This article studies the theoretical foundations of packing problems, in which the set of items is known, but the arrival order is unknown. The algorithms presented in this article can certify whether a container can hold a set of objects in the case where the arrival order is revealed at once, one-by-one, and where a limited set of objects can be put aside in a buffer before packing. These algorithms can be used to choose optimal containers and packing strategies, both for robot and human packers.

In the article, we present an efficient optimization framework that solves trajectory optimization problems by decoupling state variables from timing variables, thereby decomposing a challenging nonlinear programming (NLP) problem into two easier subproblems. With timing fixed, the state variables can be optimized efficiently using convex optimization, and the timing variables can be optimized in a separate NLP, which forms a bilevel optimization problem. The challenge of obtaining the gradient of the timing variables is solved by sensitivity analysis of parametric NLPs. The exact analytic gradient is computed from the dual solution as a by-product, whereas existing finite-difference techniques require additional optimization. The bilevel optimization framework efficiently optimizes both timing and state variables which is demonstrated on generating trajectories for an UAV. Numerical experiments demonstrate that bilevel optimization converges significantly more reliably than a standard NLP solver, and analytical gradients outperform finite differences in terms of computation speed and accuracy. Physical experiments demonstrate its real-time applicability for reactive target tracking tasks.

Time-optimal trajectories for dynamic robotic vehicles are difficult to compute even for state-of-the-art nonlinear programming (NLP) solvers, due to nonlinearity and bang-bang control structure. This paper presents a bilevel optimization framework that addresses these problems by decomposing the spatial and temporal variables into a hierarchical optimization. Specifically, the original problem is divided into an inner layer, which computes a time-optimal velocity profile along a given geometric path, and an outer layer, which refines the geometric path by a Quasi-Newton method. The inner optimization is convex and efficiently solved by interior-point methods. The gradients of the outer layer can be analytically obtained using sensitivity analysis of parametric optimization problems. A novel contribution is to introduce a duality gap in the inner optimization rather than solving it to optimality; this lets the optimizer realize warm-starting of the interior-point method, avoids non-smoothness of the outer cost function caused by active inequality constraint switching. Like prior bilevel frameworks, this method is guaranteed to return a feasible solution at any time, but converges faster than gap-free bilevel optimization. Numerical experiments on a drone model with velocity and acceleration limits show that the proposed method performs faster and more robustly than gap-free bilevel optimization and general NLP solvers.

Robotic automation has the potential to assist human surgeons in performing suturing tasks in microsurgery, and in order to do so a robot must be able to guide a needle with sub-millimeter precision through soft tissue. This paper presents a robotic suturing system that uses 3D optical coherence tomography (OCT) system for imaging feedback. Calibration of the robot-OCT and robot-needle transforms, wound detection, keypoint identification, and path planning are all performed automatically. The calibration method handles pose uncertainty when the needle is grasped using a variant of iterative closest points. The path planner uses the identified wound shape to calculate needle entry and exit points to yield an evenly-matched wound shape after closure. Experiments on tissue phantoms and animal tissue demonstrate that the system can pass a suture needle through wounds with 0.200 mm overall accuracy in achieving the planned entry and exit points, and over 20× more precise than prior autonomous suturing robots.

This paper presents a semi-empirical method for simulating contact with elastically deformable objects whose force response is learned using entirely data-driven models. A point-based surface representation and an inhomogeneous, nonlinear force response model are learned from a robotic arm acquiring force-displacement curves from a small number of poking interactions. The simulator then estimates displacement and force response when the deformable object is in contact with an arbitrary rigid object. It does so by estimating displacements by solving a Hertzian contact model, and sums the expected forces at individual surface points through querying the learned point stiffness models as a function of their expected displacements. Experiments on a variety of challenging objects show that our approach learns force response with sufficient accuracy to generate plausible contact response for novel rigid objects.

Ophthalmic microsurgery is technically difficult because the scale of required surgical tool manipulations challenge the limits of the surgeon's visual acuity, sensory perception, and physical dexterity. Intraoperative optical coherence tomography (OCT) imaging with micrometer-scale resolution is increasingly being used to monitor and provide enhanced real-time visualization of ophthalmic surgical maneuvers, but surgeons still face physical limitations when manipulating instruments inside the eye. Autonomously controlled robots are one avenue for overcoming these physical limitations. We demonstrate the feasibility of using learning from demonstration and reinforcement learning with an industrial robot to perform OCT-guided corneal needle insertions in an ex vivo model of deep anterior lamellar keratoplasty (DALK) surgery. Our reinforcement learning agent trained on ex vivo human corneas, then outperformed surgical fellows in reaching a target needle insertion depth in mock corneal surgery trials. This work shows the combination of learning from demonstration and reinforcement learning is a viable option for performing OCT guided robotic ophthalmic surgery.

Optical coherence tomography (OCT) has found great success in ophthalmology where it plays a key role in screening and diagnostics. Clinical ophthalmic OCT systems are typically deployed as tabletop instruments that require chinrest stabilization and trained ophthalmic photographers to operate. These requirements preclude OCT diagnostics in bedbound or unconscious patients who cannot use a chinrest, and restrict OCT screening to ophthalmology offices. We present a robotically-aligned OCT scanner capable of automatic eye imaging without chinrests. The scanner features eye tracking from fixed-base RGB-D cameras for coarse and stereo pupil cameras for fine alignment, as well as galvanometer aiming for fast lateral tracking, reference arm adjustment for fast axial tracking, and a commercial robot arm for slow lateral and axial tracking. We demonstrate the system's performance autonomously aligning with stationary eyes, pursuing moving eyes, and tracking eyes undergoing physiologic motion. The system demonstrates sub-millimeter eye tracking accuracy, 12 μm lateral pupil tracking accuracy, 83.2 ms stabilization time following step disturbance, and 9.7 Hz tracking bandwidth.

This paper proposes a machine learning method to predict the solutions of related nonlinear optimal control problems given some parametric input, such as the initial state. The map between problem parameters to optimal solutions is called the problem-optimum map, and is often discontinuous due to nonconvexity, discrete homotopy classes, and control switching. This causes difficulties for traditional function approximators such as neural networks, which assume continuity of the underlying function. This paper proposes a mixture of experts (MoE) model composed of a classifier and several regressors, where each regressor is tuned to a particular continuous region. A novel training approach is proposed that trains classifier and regressors independently. MoE greatly outperforms standard neural networks, and achieves highly reliable trajectory prediction (over 99.5% accuracy) in several dynamic vehicle control problems.

This paper proposes a technique for 3D object scanning via in-hand manipulation, in which an object reoriented in front of a video camera with multiple grasps and regrasps. In-hand object tracking is a significant challenge under fast movement, rapid appearance changes, and occlusions. This paper proposes a novel video-segmentation-based object tracking algorithm that tracks arbitrary in-hand objects more effectively than existing techniques. It also describes a novel RGB-D in-hand object manipulation dataset consisting of several common household objects. Experiments show that the new method achieves 6% increase in accuracy compared to top performing video tracking algorithms and results in noticeably higher quality reconstructed models. Moreover, testing with a novice user on a set of 200 objects demonstrates relatively rapid construction of complete 3D object models.

Recent progress in the field of robotic manipulation has generated interest in fully automatic object packing in warehouses. This paper proposes a formulation of the packing problem that is tailored to the automated warehousing domain. Besides minimizing waste space inside a container, the problem requires stability of the object pile during packing and the feasibility of the robot motion executing the placement plans. To address this problem, a set of constraints are formulated, and a constructive packing pipeline is proposed to solve these constraints. The pipeline is able to pack geometrically complex, non-convex objects while satisfying stability and robot packability constraints. In particular, a new 3D positioning heuristic called Heightmap-Minimization heuristic is proposed, and heightmaps are used to speed up the search. Experimental evaluation of the method is conducted with a realistic physical simulator on a dataset of scanned real-world items, demonstrating stable and high-quality packing plans compared with other 3D packing methods.

In this paper, we propose a semi-empirical approach for simulating robot locomotion on granular media. We first develop a contact model based on the stick-slip behavior between rigid objects and granular grains, which is then learned through running extensive experiments. The contact model represents all possible contact wrenches that the granular substrate can provide as a convex volume, which our method formulates as constraints in an optimization-based contact force solver. During simulation, granular substrates are treated as rigid objects that allow penetration and the contact solver solves for wrenches that maximize frictional dissipation. We show that our method is able to simulate plausible interaction response with several granular media at interactive rates.

This paper presents a general framework to find time-optimal trajectories for dynamic vehicles like drones and autonomous cars. Hindered by its nonlinear objective and complex constraints, this problem is hard even for state-of the-art nonlinear programming (NLP) solvers. The proposed framework addresses the problem by bilevel optimization. Specifically, the original problem is divided into an inner layer, which computes a time-optimal velocity profile along a fixed geometric path, and an outer layer, which refines the geometric path by a Quasi-Newton method. The inner optimization is convex and efficiently solved by interior-point methods. A novel variable reordering method is introduced to accelerate the optimization of the velocity profile. The gradients of the outer layer can be derived from the Lagrange multipliers using sensitivity analysis of parametric optimization problems. The method is guaranteed to return a feasible solution at any time, and numerical experiments on a ground vehicle with friction circle dynamics model show that the proposed method performs more robustly than general NLP solvers.

This paper presents a Bayesian approach for active tactile exploration of a planar shape in the presence of both localization and shape uncertainty. The goal is to dock the robot's end-effector against the shape-reaching a point of contact that resists a desired load-with as few probing actions as possible. The proposed method repeatedly performs inference, planning, and execution steps. Given a prior probability distribution over object shape and sensor readings from previously executed motions, the posterior distribution is inferred using a novel and efficient Hamiltonian Monte Carlo method. The optimal docking site is chosen to maximize docking probability, using a closed-form probabilistic simulation that accepts rigid and compliant motion models under Coulomb friction. Numerical experiments demonstrate that this method requires fewer exploration actions to dock than heuristics and information-gain strategies.

Deep anterior lamellar keratoplasty (DALK) is a promising technique for corneal transplantation that avoids the chronic immunosuppression comorbidities and graft rejection risk associated with penetrating keratoplasty (PKP), the standard procedure. In DALK, surgeons must insert a needle 90% through the 500 μm cornea without penetrating its underlying membrane. This pushes surgeons to their manipulation and visualization limits such that 59% of DALK attempts fail due to corneal perforation or inadequate needle depth. We propose a robot-assisted solution to jointly solve the manipulation and visualization challenges using a cooperatively-controlled, precise robot arm and live optical coherence tomography (OCT) imaging, respectively. Our system features an interface handle, with which the surgeon and robot cooperatively hold the tool, and a posterior corneal boundary virtual fixture driven by real-time OCT segmentation. A study in which three operators performed DALK needle insertions manually and cooperatively in ex vivo human corneas demonstrated an 84% improvement in perforation-free needle depth without an increased perforation rate.

In order to conduct many desirable functions, service robots will need to actuate buttons and switches that are designed for humans. This paper presents the design of a robot named SwitchIt that is small, relatively inexpensive, easily mounted on a mobile robot, and actuates buttons reliably. Its operating characteristics were developed after conducting a systematic study of buttons and switches in human environments. From this study, we develop a categorization of buttons based on a set of physical properties relevant for robots to operate them. After a human calibrates and annotates buttons in the robot's environment using a hand-held tablet, the system automatically recognizes, pushes, and detects the state of a variety of buttons. Empirical tests demonstrate that the system succeeds in operating 95.7% of 234 total buttons/switches in an office building and a household environment.

In this paper, we present a real-time falling robot stabilization system for a humanoid robot in which the robot can prevent falling using hand contact with walls and other surfaces in the environment. Instead of ignoring or avoiding interaction with environmental obstacles, our system uses obstacle geometry to determine a contact point that reduces impact and necessary friction. It uses a planar dynamic model that is appropriate for falling stabilization in the robot's sagittal plane and frontal plane. The hand contact is determined with an optimal control approach, and to make the algorithm run in realtime, a simplified three-link robot model and a pre-computed database of subproblems for the hand contact optimization are adopted. Moreover, if the robot is not leaning too far after stabilization, we employ a heuristic push-up strategy to recover the robot to a standing posture. System integration is performed on the Darwin-Mini robot and validation is conducted in several environments and falling scenarios.

Walking and climbing robots need to plan longterm routes on both horizontal and vertical terrain, but onboard sensors take images from vantage points that provide strongly foreshortened images that cause the appearance of terrain features to vary greatly by distance and viewing angle. This paper presents a convolutional neural network (CNN) method for predicting valid handhold and foothold locations from single RGB+D images taken at arbitrary tilt angles. Experiments show that the method predicts holds more accurately than comparable learning techniques, and that a route planner based on these predictions generates plausible plans for flat ground, stairs, and walls in rock climbing gyms.

Nonlinear optimal control problems are challenging to solve efficiently due to non-convexity. This paper introduces a trajectory optimization approach that achieves realtime performance by combining machine learning to predict optimal trajectories with refinement by quadratic optimization. First, a library of optimal trajectories is calculated offline and used to train a neural network. Online, the neural network predicts a trajectory for a novel initial state and cost function, and this prediction is further optimized by a sparse quadratic programming solver. We apply this approach to a fly-to-target movement problem for an indoor quadrotor. Experiments demonstrate that the technique calculates near-optimal trajectories in a few milliseconds, and generates agile movement that can be tracked more accurately than existing methods.

Robot-to-human intent communication has been proposed as a method of enabling fluent coordination in human-robot teams. Prior research has focused on identifying modalities by which intent information can be accurately communicated, but has not yet studied whether intent communication enables fluent or safer coordination in human-robot teams in which intent communication is only supportive to the team's primary task. To address this question, we conduct a study (N = 29) in a mock collaborative manufacturing scenario in which motion-based and display-based intent communication approaches are evaluated under varying penalties for failing to coordinate safely. Subjective and objective measures of team fluency suggest that although intent communication supports fluent coordination, using a purely motion-based or a purely display-based approach may not be the most effective strategy. Although multimodal intent communication did not significantly improve upon unimodal approaches, merging both motion-based and display-based intent communication seems to combine the strengths of both approaches. Interestingly, results also suggest that contrary to theoretical predictions, the positive effect of intent communication is generally robust to teaming scenarios that require members to operate concurrently.

This paper presents an overview of the inaugural Amazon Picking Challenge along with a summary of a survey conducted among the 26 participating teams. The challenge goal was to design an autonomous robot to pick items from a warehouse shelf. This task is currently performed by human workers, and there is hope that robots can someday help increase efficiency and throughput while lowering cost. We report on a 28-question survey posed to the teams to learn about each team's background, mechanism design, perception apparatus, planning, and control approach. We identify trends in this data, correlate it with each team's success in the competition, and discuss observations and lessons learned based on survey results and the authors' personal experiences during the challenge.

This paper presents a novel approach to kinematic redundancy resolution for redundant robots, which have more degrees of freedom than workspace dimensions. It introduces the concept of a global redundancy resolution, which has the convenient property that whenever the robot returns to the same workspace point, it uses the same joint-space pose. The problem is cast, as a continuous pseudoinversion of the forward kinematic map. Continuity and smoothness should be attained if possible, but otherwise the volume of the discontinuity boundary should be minimized. A sampling-based approximation technique is presented that constructs roadmaps of both the domain and image, and minimizes discontinuities of the inverse function using a maximum satisfiability problem. Applications of this map include teleoperation, dimensionality reduction in motion planning, and workspace visualization. Results are demonstrated on toy problems with up to 20 DOF and on several robot arms.

This paper presents a method for efficiently testing the stability of an object under contact that accommodates empirical models of admissible forces at individual contact points. It handles a diverse range of possible geometries of the admissible force volume, including anisotropy, adhesion, and even nonconvexity. The method discretizes the contact region into patches, performs a convex decomposition of a polyhedral approximation to each admissible force volume, and then formulates the problem as a mixed integer linear program. The model can also accommodate articulated robot hands with torque limits and joint frictions. Predictions of our method are evaluated experimentally in object lifting tasks using a gripper that exploits microspines to exert strongly anisotropic forces. The method is applied to calculate gripper loading capabilities and equilibrium predictions for a quadruped climbing robot on steep and overhanging terrain.

During outbreaks of contagious diseases, healthcare workers are at high risk for infection due to routine interaction with patients, handling of contaminated materials, and challenges associated with safely removing protective gear. This poses an opportunity for the use of remote-controlled robots that could perform common nursing duties inside hazardous clinical areas, thereby minimizing the exposure of healthcare workers to contagions and other biohazards. This paper describes the development of the prototype system Tele-Robotic Intelligent Nursing Assistant (TRINA), which consists of a mobile manipulator robot, a human operator's console, and operator assistance algorithms which automate or partially-automate tedious and error-prone tasks. Using off-the-shelf robotic and sensing components, total hardware costs are kept under $75,000. The system's capabilities for performing standard nursing tasks are evaluated in the simulation laboratory of a nursing school.

The addition of manipulation capabilities to telepresence robots holds the promise of enabling remote humans to perform tele-labor, hands-on training, and collaborative manipulation, but the use of a robot as a mediator to humanhuman physical interaction is not yet well understood. This paper studies the impact of telepresence modalities in the context of robot-mediated object handover. A teleoperation system was developed involving a bimanual mobile manipulator with telepresence head and sensing capabilities, and a user study was conducted with n=10 pairs of subjects under a variety of audio and visual telepresence conditions. Results show that telepresence does not significantly affect objective handover fluency, but both audio and video telepresence do significantly improve user experience on subjective measures including intimacy and perceived fluency.

Differential dynamic programming (DDP) is a widely used trajectory optimization technique that addresses nonlinear optimal control problems, and can readily handle nonlinear cost functions. However, it does not handle either state or control constraints. This paper presents a novel formulation of DDP that is able to accommodate arbitrary nonlinear inequality constraints on both state and control. The main insight in standard DDP is that a quadratic approximation of the value function can be derived using a recursive backward pass, however the recursive formulae are only valid for unconstrained problems. The main technical contribution of the presented method is a derivation of the recursive quadratic approximation formula in the presence of nonlinear constraints, after a set of active constraints has been identified at each point in time. This formula is used in a new Constrained-DDP (CDDP) algorithm that iteratively determines these active set and is guaranteed to converge toward a local minimum. CDDP is demonstrated on several underactuated optimal control problems up to 12D with obstacle avoidance and control constraints and is shown to outperform other methods for accommodating constraints.

Convolutional neural networks (CNN) are a deep learning technique that has achieved state-of-the-art prediction performance in computer vision and robotics, but assume the input data can be formatted as an image or video (e.g. predicting a robot grasping location given RGB-D image input). This paper considers the problem of augmenting a traditional CNN for handling image-like input (called main-channel input) with additional, highly predictive, non-image-like input (called side-channel input). An example of such a task would be to predict whether a robot path is collision-free given an occupancy grid of the environment and the path's start and goal configurations; the occupancy grid is the main-channel and the start and goal are the side-channel. This paper presents several candidate network architectures for doing so. Empirical tests on robot collision prediction and control problems compare the proposed architectures in terms of learning speed, memory usage, learning capacity, and susceptibility to overfitting.

Intraoperative 3D imaging has great potential for enhancing surgical visualization. This is especially so in ophthalmic surgery where live volumetric imaging from optical coherence tomography systems recently incorporated into surgical microscopes has freed surgeons from the otherwise universal top-down viewpoint. New viewpoints, however, disorient surgeons when directions of their hand motions and viewed tool motions do not align. We propose introducing a robotic surgery system to decouple surgeons' hands from their tools and ensure that viewed tool motions align in arbitrary viewpoints. We present a framework entitled Arbitrary Viewpoint Robotic Manipulation (AVRM) which governs how hand and tool motions should interact to minimize disorientation and thereby enable operations from desirable but previously untenable viewpoints. A crossover study in which 20 subjects completed mock surgical scenarios with an AVRM testbed system demonstrated that arbitrary viewpoints do not improve task performance unless automatic hand-tool misalignment correction is provided. When provided together with arbitrary viewpoints, automatic hand-tool misalignment correction reduces task completion time by 50% on average compared to a fixed top-down viewpoint.

Nonlinear optimal control problems are challenging to solve due to the prevalence of local minima that prevent convergence and/or optimality. This paper describes nearest-neighbors optimal control (NNOC), a data-driven framework for nonlinear optimal control using indirect methods. It determines initial guesses for new problems with the help of precomputed solutions to similar problems, retrieved using k-nearest neighbors. A sensitivity analysis technique is introduced to linearly approximate the variation of solutions between new and precomputed problems based on their variation of parameters. Experiments show that NNOC can obtain the global optimal solution orders of magnitude faster than standard random restart methods, and sensitivity analysis can further reduce the solving time almost by half. Examples are shown on two optimal control problems in vehicle control.

Despite increasing popularity of compliant and underactuated hands, few tools are available for modeling them. Thus, we propose a simulation technique to predict the success of a compliant gripper grasping irregular objects, which could be used in mechanism design as well as grasp planning. The simulator we propose integrates joint compliance simulation with a Boundary Layer Expanded Mesh (BLEM) technique to enhance the stability of contact estimation. We compare the proposed simulator with existing simulators via a set of stability and fidelity criteria, including contact force variation, contact position variation, and contact normal variation. Scores along these criteria are correlated with the simulator's accuracy of predicting the success/failure of a given grasp pose and preshape. A test set of 13 grasps, with two compliant underactuated hands were manually generated on 4 objects. Experiments suggest that our simulator leads to improvements in the stability criteria, predictability of grasp success, and reduction of simulation artifacts.

This paper presents an equivalence between feasible kinodynamic planning and optimal kinodynamic planning, in that any optimal planning problem can be transformed into a series of feasible planning problems in a state-cost space, whose solutions approach the optimum. This transformation yields a meta-algorithm that produces an asymptotically optimal planner, given any feasible kinodynamic planner as a subroutine. The meta-algorithm is proven to be asymptotically optimal and a formula is derived relating expected running time and solution suboptimality. It is directly applicable to a wide range of optimal planning problems because it does not resort to the use of steering functions or numerical boundary-value problem solvers. On a set of benchmark problems, it is demonstrated to perform, using the expansive space tree (EST) and rapidly-exploring random tree (RRT) algorithms as subroutines, at a level that is superior or comparable to related planners.

Asymptotically-optimal sampling-based motion planners, like RRT*, perform vast amounts of collision checking, and are hence rather slow to converge in complex problems where collision checking is relatively expensive. This paper presents two novel motion planners, Lazy-PRM* and Lazy-RRG*, that eliminate the majority of collision checks using a lazy strategy. They are sampling-based, any-time, and asymptotically complete algorithms that grow a network of feasible vertices connected by edges. Edges are not immediately checked for collision, but rather are checked only when a better path to the goal is found. This strategy avoids checking the vast majority of edges that have no chance of being on an optimal path. Experiments show that the new methods converge toward the optimum substantially faster than existing planners on rigid body path planning and robot manipulation problems.

This paper describes a clinical decision support framework in multi-step health care domains that can dynamically recommend optimal treatment plans with respect to both patient outcomes and expected treatment cost. Our system uses a modified POMDP framework in which hidden states are not explicitly modeled, but rather, probabilistic models for predicting future observables given observation and action histories are learned directly from electronic health record (EHR) data. High quality treatment recommendations are found using a sampling-based tree growing approach which produces good results despite only exploring a fraction of the observation and action spaces. We describe the application of the approach to an ischemic stroke domain with clinical trial data (International Stroke Trial Dataset, 1993–1996). The dataset is of moderate size (N=19,435) and exhibits many characteristics of real EHR data, including noise, missing values, and idiosyncratic coding. The system's predictive model was chosen using cross-validated model selection from a set of several candidate learning methods, including logistic regression, Naïve Bayes, Bayes nets, and random forests. Simulations suggest that the optimized decisions improve patient outcomes, such as 6-month survival rate, compared to the decisions of human doctors during the study.

Intelligent behavior in robots is implemented through algorithms. Historically, much of algorithmic robotics research strives to compute outputs that achieve mathematically rigid conditions, such as minimizing path length. But today's robots are increasingly being used to empower the daily lives of people, and experience shows that traditional algorithmic approaches are poorly suited for the unpredictable, idiosyncratic, and adaptive nature of human-robot interaction. This raises a need for entirely new computational, mathematical, and technical approaches for robots to better understand and react to humans. The human-friendly robots of the future will need new algorithms, informed from the ground up by HRI research, to generate interpretable, ethical, socially-acceptable behavior, ensure safety around humans, and execute tasks of value to society.

We consider the problem of learning probabilistic models from relational data. One of the key issues with relational data is class imbalance where the number of negative examples far outnumbers the number of positive examples. The common approach for dealing with this problem is the use of sub-sampling of negative examples. We, on the other hand, consider a soft margin approach that explicitly trades off between the false positives and false negatives. We apply this approach to the recently successful formalism of relational functional gradient boosting. Specifically, we modify the objective function of the learning problem to explicitly include the trade-off between false positives and negatives. We show empirically that this approach is more successful in handling the class imbalance problem than the original framework that weighed all the examples equally.

This paper presents a probabilistic technique for identifying support surfaces like floors, walls, stairs, and rails from unstructured 3D point cloud scans. A Markov random field is employed to model the joint probability of point labels, which can take on a number of user-defined surface classes. The probability of a point depends on both local spatial features of the point cloud around the point as well as the classifications of points in its neighborhood. The training step estimates joint and pairwise potentials from labeled point cloud datasets, and the prediction step aims to maximize the joint probability of all labels using a hill-climbing procedure. The method is applied to stair and ladder detection from noisy and partial scans using three types of sensors: a sweeping laser sensor, time-offlight depth camera, and a Kinect depth camera. The resulting classifier achieves approximately 75% accuracy and is robust to variations in point density.

This paper presents an algorithm for minimizing the execution time of a geometric robot path while satisfying dynamic force and torque constraints. The formulation is numerically stable, using a convex optimization that is guaranteed to converge to a unique optimum, and it is also scalable due to the use of a fast feasible set precomputation step that greatly reduces dimensionality of the optimization problem. The algorithm handles frictional contact constraints with arbitrary numbers of contact points as well as torque, acceleration, and velocity limits. Results are demonstrated in simulation on locomotion problems on the Hubo-II+ and ATLAS humanoid robots, demonstrating that the algorithm can optimize trajectories for robots with dozens of degrees of freedom and dozens of contact points in a few seconds.

This paper presents an autonomous planning and control framework for humanoid robots to climb general ladder- and stair-like structures. The approach consists of two major components: 1) a multi-limbed locomotion planner that takes as input a ladder model and automatically generates a whole-body climbing trajectory that satisfies contact, collision, and torque limit constraints; 2) a compliance controller which allows the robot to tolerate errors from sensing, calibration, and execution. Simulations demonstrate that the robot is capable of climbing a wide range of ladders and tolerating disturbances and errors. Physical experiments demonstrate the DRC-Hubo humanoid robot successfully mounting, climbing, and dismounting an industrial ladder similar to the one intended to be used in the DARPA Robotics Challenge Trials.

This video presents our preliminary work towards addressing the ladder climbing event in DARPA Robotics Challenge (DRC) using DRC-Hubo robot. A ladder-climbing motion planner is developed which generates a collision-free, stable quasi-static trajectory for execution. Compliance control is enabled on arm joints to compensate for the calibration error, modeling error and control error. We have demonstrated that DRC-Hubo can robustly climb a variety of ladders in simulation and successfully climb a ship ladder on the hardware.

ROBOPuppet is a method to create inexpensive, tabletop-sized robot models to provide teleoperation input to full-sized robots. It provides a direct physical correspondence from the device to the robot, which is appealing because users form an immediate “mental mapping” of the input-output behavior. We observe that untrained users can immediately exploit tactile and physical intuition when controlling the puppet to perform complex actions the target robot. The key contribution of this paper is a build procedure that embeds standardized encoder modules into scaled-down CAD models of the robot links, which are then 3D printed and assembled. This procedure is generalizable to variety of robots, and parts cost approximately seventeen dollars per link. We also present a simple software tool for fast calibration of the puppet-robot mapping, and a safety filtering procedure that sanitizes the noisy inputs so that the robot avoids collisions and satisfies dynamic constraints. A prototype ROBOPuppet is built for a 6DOF industrial manipulator and tested in simulation and on the physical robot.

This paper presents a systematic benchmarking comparison between optimal motion planners. Six planners representing the categories of sampling-based, grid-based, and trajectory optimization methods are compared on synthetic problems of varying dimensionality, number of homotopy classes, and width and length of narrow passages. Performance statistics are gathered on success and convergence rates, and performance variations with respect to geometric characteristics are analyzed. Based on this analysis, we recommend planners that are likely to perform well for certain problem classes, and make recommendations for future planning research.

This paper presents a Monte Carlo technique for sampling configurations of a kinematic chain according to a specified probability density while accounting for loop closure constraints. A key contribution is a method for sampling sub-loops in unbiased fashion using analytical inverse kinematics techniques. Sub-loops are then iterated across the chain to produce samples for the entire chain. The method is demonstrated to scale well to high-dimensional chains (>200DOFs) and is applied to flexible 2D chains, protein molecules, and robots with multiple closed-chains.

At intersections and in merging traffic, intelligent road vehicles must solve challenging optimal control problems in real-time to navigate reliably around moving obstacles. We present a complete planner that computes collision-free, optimal longitudinal control sequences (acceleration and braking) using a novel visibility graph approach that analytically computes the reachable subset of path-velocity-time space. We demonstrate that our method plans over an order of magnitude faster than previous approaches, making it scalable and fast enough (tenths of a second on a PC) to be called repeatedly on-line. We demonstrate applications to autonomous driving and vehicle collision warning systems with many moving obstacles.

Vehicles that cross lanes of traffic encounter the problem of navigating around dynamic obstacles under actuation constraints. This paper presents an optimal, exact, polynomial-time planner for optimal bounded-acceleration trajectories along a fixed, given path with dynamic obstacles. The planner constructs reachable sets in the path-velocity-time (PVT) space by propagating reachable velocity sets between obstacle tangent points in the path-time (PT) space. The terminal velocities attainable by endpoint-constrained trajectories in the same homotopic class are proven to span a convex interval, so the planner merges contributions from individual homotopic classes to find the exact range of reachable velocities and times at the goal. A reachability analysis proves that running time is polynomial given reasonable assumptions, and empirical tests demonstrate that it scales well in practice and can handle hundreds of dynamic obstacles in a fraction of a second on a standard PC.

This paper presents a method for generating dynamically-feasible, natural-looking robot motion from freehand sketches. Using trajectory optimization, it handles sketches that are too fast, jerky, or pass out of reach by enforcing the constraints of the robot's dynamic limitations while minimizing the relative temporal differences between the robot's trajectory and the sketch. To make the optimization fast enough for interactive use, a variety of enhancements are employed including decoupling the geometric and temporal optimizations and methods to select good initial trajectories. The technique is also applicable to transferring human motions onto robots with non-human appearance and dynamics, and we use our method to demonstrate a simulated humanoid imitating a golf swing as well as an industrial robot performing the motion of writing a cursive “hello” word.

Dynamic manipulations require attaining high velocities at specified configurations, all the while obeying geometric and dynamic constraints. This paper presents a motion planner that constructs a trajectory that passes at an intermediate state through a dynamic objective region, which is comprised of a certain lower dimensional submanifold in the configuration/velocity state space, and then returns to rest. Planning speed and reliability are greatly improved by finding good intermediate states first, because the choice of intermediate state couples the ramp-up and ramp-down subproblems, and moreover very few (often less than 1%) intermediate states yield feasible solution trajectories. Simulation experiments demonstrate that our method quickly generates trajectories for a 6-DOF industrial manipulator throwing a small object.

This paper considers a shortcutting heuristic to smooth jerky trajectories for many-DOF robot manipulators subject to collision constraints, velocity bounds, and acceleration bounds. The heuristic repeatedly picks two points on the trajectory and attempts to replace the intermediate trajectory with a shorter, collision-free segment. Here, we construct segments that interpolate between endpoints with specified velocity in a time-optimal fashion, while respecting velocity and acceleration bounds. These trajectory segments consist of parabolic and straight-line curves, and can be computed in closed form. Experiments on reaching tasks in cluttered human environments demonstrate that the technique can generate smooth, collision-free, and natural-looking motion in seconds for a PUMA manipulator and the Honda ASIMO robot.

We report our first experiences with Leaving Flatland, an exploratory project that studies the key challenges of closing the loop between autonomous perception and action on challenging terrain. We propose a comprehensive system for localization, mapping, and planning for the RHex mobile robot in fully 3D indoor and outdoor environments. This system integrates Visual Odometry-based localization with new techniques in real-time 3D mapping from stereo data. The motion planner uses a new decomposition approach to adapt existing 2D planning techniques to operate in 3D terrain. We test the map-building and motion-planning subsystems on real and synthetic data, and show that they have favorable computational performance for use in high-speed autonomous navigation.

This paper addresses the use of robotic tissue manipulation in medical needle insertion procedures to improve targeting accuracy and to help avoid damaging sensitive tissues. To control these multiple, potentially competing objectives, we present a phased controller that operates one manipulator at a time using closed-loop imaging feedback. We present an automated procedure planning technique that uses tissue geometry to select the needle insertion location, manipulation locations, and controller parameters. The planner uses a stochastic optimization of a cost function that includes tissue stress and robustness to disturbances. We demonstrate the system on 2D tissues simulated with a mass-spring model, including a simulation of a prostate brachytherapy procedure. It can reduce targeting errors from more than 2 cm to less than 1 mm, and can also shift obstacles by over 1 cm to clear them away from the needle path.

This paper considers robotic automation of a common surgical retraction primitive of exposing an underlying area by grasping and lifting a thin, 3D, possibly inhomogeneous layer of tissue. We present an algorithm that computes a set of stable and secure grasp-and-retract trajectories for a point-jaw gripper moving along a plane, and runs a 3D finite element (FEM) simulation to certify and assess the quality of each trajectory. To compute secure candidate grasp locations, we use a continuous spring model of thin, inhomogeneous deformable objects with linear energy potential. Experiments show that this method produces many of the same grasps as an exhaustive optimization with an FEM mesh, but is orders of magnitude cheaper: our method runs in O(v log v) time, where v is the number of veins, while the FEM computation takes O(pn3) time, where n is the number of nodes in the FEM mesh and p is the number of nodes on its perimeter. Furthermore, we present a constant tissue curvature (CTC) retraction trajectory that distributes strain uniformly around the medial axis of the tissue. 3D FEM simulations show that the CTC achieves retractions with lower tissue strain than circular and linear trajectories. Overall, our algorithm computes and certifies a high-quality retraction in about one minute on a PC.

We present algorithms for simulating and visualizing the insertion and steering of needles through deformable tissues for surgical training and planning. Needle insertion is an essential component of many clinical procedures such as biopsies, injections, neurosurgery, and brachytherapy cancer treatment. The success of these procedures depends on accurate guidance of the needle tip to a clinical target while avoiding vital tissues. Needle insertion deforms body tissues, making accurate placement difficult. Our interactive needle insertion simulator models the coupling between a steerable needle and deformable tissue. We introduce (1) a novel algorithm for local remeshing that quickly enforces the conformity of a tetrahedral mesh to a curvilinear needle path, enabling accurate computation of contact forces, (2) an efficient method for coupling a 3D finite element simulation with a 1D inextensible rod with stick-slip friction, and (3) optimizations that reduce the computation time for physically based simulations. We can realistically and interactively simulate needle insertion into a prostate mesh of 13,375 tetrahedra and 2,763 vertices at a 25 Hz frame rate on an 8-core 3.0 GHz Intel Xeon PC. The simulation models prostate brachytherapy with needles of varying stiffness, steering needles around obstacles, and supports motion planning for robotic needle insertion. We evaluate the accuracy of the simulation by comparing against real-world experiments in which flexible, steerable needles were inserted into gel tissue phantoms.

This paper presents a method of generating natural-looking motion primitives for humanoid robots. An optimization-based approach is used to generate these primitives, but the objective function is tailored to each one and complexity is reduced by identifying relevant degrees of freedom. Several examples are shown in simulation: for an arm movement to reach an object, it is better to minimize the acceleration of key parts of the robot over its entire trajectory; for a single step on flat ground, it is better to minimize the torque and instantaneous angular momentum at every posture. The primitives are precomputed off-line, but might be used by on-line planner either to provide a fixed set of maneuvers or to bias a probabilistic, sample-based search for motions

Probabilistic sampling-based motion planners are unable to detect when no feasible path exists. A common heuristic is to declare a query infeasible if a path is not found in a fixed amount of time. In applications where many queries must be processed – for instance, robotic manipulation, multi-limbed locomotion, and contact motion – a critical question arises: what should this time limit be? This paper presents a machine-learning approach to deal with this question. In an off-line learning phase, a classifier is trained to quickly predict the feasibility of a query. Then, an improved multi-step motion planning algorithm uses this classifier to avoid wasting time on infeasible queries. This approach has been successfully demonstrated in simulation on a four-limbed, free-climbing robot.