This article presents work of a research & development effort that focuses on realizing effective human-supervised humanoid robot control for non-conventional emergency response. The devastation and dangerous operational environments caused by natural and man-made disasters have been the motivation for the DARPA Robotics Challenge (DRC). Humanoid robots have advantages for completing a wide variety of tasks in human environments such as opening doors, turning valves, operating power tools, traversing rough terrain and driving a vehicle. This article also discusses various approaches, results, and lessons learned. Meeting the requirements of the DRC tasks using an Atlas humanoid robot has been the main research and development effort. One of the lessons learnt is that providing effective and easy to use human-robot interfaces is critical. Since the DRC poses realistically challenging operating conditions for the human, the interfaces need to have sufficient autonomy to minimize human errors.

## Article

The devastation and dangerous operational environments caused by natural and man-made disasters have been the motivation for the DARPA Robotics Challenge (DRC). What if we had been able to prevent hydrogen explosions in the Fukushima Daiichi Nuclear Power Plant by using robots within the first hour after it was hit by a tsunami triggered by the Great East Japan earthquake in 2011? For the past three years, the DRC has mobilized hundreds of robotics researchers, practitioners and makers to accelerate the research and development in robotics for disaster response. The DRC Finals on June 5-6, 2015 bring 25 qualified teams to Pomona, CA to demonstrate their systems in a disaster mission scenario. In a simulated environment, robots perform a variety of manipulation and mobility tasks. The robots operate under human supervision with a 1-hour mission completion time. Communication between the operator(s) and robot is degraded during the mission to promote robot autonomy.

Humanoid robots have advantages for completing a wide variety of tasks in human environments such as opening doors, turning valves, operating power tools, traversing rough terrain and driving a vehicle. However, despite receiving great attention to date, humanoid robot motion planning and control remain challenging research topics. Completion of the DRC mission with a humanoid robot requires the development of reliable and accurate techniques for perception, full-body motion planning and control, and dexterous manipulation as well as operator training.

The Worcester Polytechnic Institute (WPI)-Carnegie Mellon University (CMU) DRC team, originally known as WPI Robotics Engineering C Squad (WRECS) which took 2nd place in the Virtual Robotics Challenge in June 2013, participated in the DRC Trials as the only Track C team in December 2013. Team WPI-CMU scored 11 out of a possible 32 points, ranked 7th (out of 16) at the DRC Trials, and was selected as a finalist for the DRC Finals. Our team is preparing to participate in the DRC Finals at the time of writing this article. More than the points earned, developing new human-supervised control techniques for the Atlas humanoid robot to complete disaster response tasks has been the focus of our work in the past three years. In this article, we present our approaches, results and lessons learned. We will start by describing the robot hardware for the sake of completeness.

## Atlas Unplugged

In July 2013, Team WPI-CMU was provided with an Atlas humanoid robot, designed and built by Boston Dynamics specifically for the DRC. Atlas is a 150 kg humanoid robot with 28 hydraulically actuated degrees of freedom (DOF): 6 in each arm, 6 in each leg, 3 at the torso, and 1 in the neck. The form factor and the anthropomorphic design of the robot make it suitable to work in human environments and operate tools specifically designed for human use. In addition to load cells for force sensing at hands and feet and a fiber-optic inertial measurement unit (IMU) at the pelvis for estimating robot pose, each actuator on the arms has a linear potentiometer for position measurement and two pressure sensors to determine the joint forces based on differential pressure measurements.

The robot's sensor suite also includes three IP (Ethernet) cameras to allow for a near 360̊ view of its surroundings and a Carnegie Robotics MultiSense SL sensor head which provides visual input to the operator. The MultiSense SL contains a set of stereovision cameras and a rotating LIDAR and can be used to produce a point-cloud to represent the robot view. The DARPA-developed Atlas robot has been upgraded in early 2015 to include a battery pack for on-board power and a new pump system. Atlas's upgrades also include new electrically actuated forearms with an additional wrist joint for improved dexterity (Figure 1). Even though the robot is designed to run untethered and without a safety line in the DRC Finals, in our laboratory experiments, the power is provided by a tether from a 480VAC supply. The Atlas is both mechanically and computationally powerful. It is equipped with three on board perception computers and a Wi-Fi link to a field computer for data processing. A Degraded Communications Emulator connects the Operator Control Stations to the robot through the field computer to emulate realistic signal-loss conditions during disasters. Team WPI-CMU's Atlas robot is equipped with three-fingered Robotiq hands that can be position, speed or force controlled. This selection is as a result of our detailed comparative study of three robotic hands provided to our team by DARPA [1].

Meeting the requirements of the DRC tasks using an Atlas humanoid robot has been the main research and development effort for our team. In this article, we highlight three aspects of human supervised control of the Atlas for non-conventional disaster response, the development of an optimization based full-body controller developed by our team, our model-based software design methodology for task completion and our approach to factoring tasks between the human operator and robot to maximize the utility of the humanrobot team.

## Full-Body Control

At the DRC Trials in December 2013, the rough terrain task consisted of walking over inclines and then piles of cinder blocks, including tilted cinder blocks. In our initial tests, the walking and step controllers from Boston Dynamics were able to walk over much of the terrain, but not all of it. We therefore decided to develop our own walking controller [2]. Our analysis of the DRC Trials terrain task was that it was a “stepping stone” task, in that it required accurate foot placement. The robot's feet needed to be placed within the boundary of individual cinder blocks. We therefore developed a walking controller that focused on achieving known footstep targets, while footstep plans were automatically generated.

The full-body controller is implemented for the Atlas robot using quadratic programming to perform inverse dynamics and inverse kinematics. For each task, desired Cartesian motions for specific locations on the robot (e.g., foot, hand, and CoM) in the high-level controller are specified. The low-level controller takes these motions as inputs and computes physical quantities for each individual joint such as joint position, velocity, acceleration, and torque. Some of these outputs are then used as references in the joint level servos on the robot. Both inverse kinematics and inverse dynamics are formulated as quadratic programming problems, whose general form is given by
$minχ 0.5χTGχ+gTχ,s.t.CEχ+cE=0,CIχ+cI≥0.$
where X is the unknown and CE, cE, CI, cI are constraint coefficients which are problem specific.

The full-body controller was originally targeted at rough terrain bipedal walking and it has been redesigned to handle ladder climbing and full body manipulation. We also developed a state estimator to estimate pelvis translational velocity and Cartesian position. We used the IMU orientation estimate directly. Based on which foot was on the ground, we used leg kinematics to provide us with a “measurement” of pelvis velocity and position. We used a simple Kalman filter to process this information.

The controller for ladder climbing at the DRC Trials was similar to our controllers for rough terrain walking and full body manipulation. The high-level control was provided by a manually generated script that implemented a quadruped climbing gait. The robot climbed one tread at a time. First the arms grasped a tread. Then one foot moved up one tread, followed by the other foot moving to the same tread. One arm moved up one tread, followed by the other arm. The DRC Finals will have a similar stair climbing task (Figure 2) which can now be completed using our walking controller.

One of our key decisions at the DRC Trials was to insert opportunities for a human operator to supervise hand and foot placement. In essence, the operator can position the robot limbs using small end-effector motions based on user keyboard inputs, called nudges.

Nudges are 1 cm translations that can be commanded by the operator, moving the end-effector in the specified direction for effective human supervised step planning and manipulation. The robot would look at where the limb was supposed to go. The limb would move near to its target. The operator could use the keyboard to precisely place the limb with 1 cm increments horizontally, using visual feedback from the robot's cameras. This strategy worked quite well at the DRC Trials. Since then, for the full-body manipulation, we incorporated depth cameras at the robot wrists and implemented a visual servoing technique for manipulation tasks that require precision such as valve turning, operating a drill and door opening.

## Model-Based Design

We adopt a model-based design (MBD) approach in our software development for task completion. MBD is a powerful design technique that emphasizes mathematical modeling to design, analyze, verify and validate complex dynamic systems in which physical processes and computation are tightly integrated. In completing disaster relevant manipulation and mobility tasks for the DRC, we use an iterative MBD approach to specify and verify requirements for each task, develop models for physical human and robot actions as well as the environment, select and compose models of computation, simulate the human-supervised system as a whole, and verify and validate the algorithm designs on the physical robot. To illustrate this approach we will focus on the DRC Door Task which is a key task to complete to enter the building and attempt all other manipulation tasks in the DRC Finals mission scenario.

We split the Door Task into four sub-tasks; door detection (DoorDetect), approach to the door (Approach), door opening (Open), and walk through the door (GoThrough). To maximize the utility of the human-robot team to complete this task, we developed a strategy for the door task in the DRC Trials. The key aspect to note is the factoring of the task between the human operator and robot as depicted in Figure 3. This factoring leverages the superior perception and decision-making capabilities of the human operator to oversee the feasibility of the steps planned in walking, selecting one of the three door types, and enabling the operator to make adjustments to robot actions to minimize errors. In the meantime, robot intelligence handles the balancing, motion planning and detection algorithms.For the computation model, an event-driven finite state machine (FSM) with the sub-tasks as the states is used to control the autonomous execution of the process with human supervision and validation at critical steps. The first state in the FSM is DoorDetect which performs the door detection using a combined 2D and 3D segmentation technique to achieve successful detection of the door based on the geometric features. There is an option to do manual human detection if the algorithm does not converge in a certain time period. Once the normal vector to the door and the position of the door handle are calculated, FSM transitions to the Approach state. In this state, the robot follows a stepping trajectory generated by an A* planner and walks to the desired stand position for opening the door. The transition to Open state occurs when the robot fully executes the planned trajectory and comes to a stop in front of the door. The Open state is an FSM on its own and this structure demonstrates our approach to composition of the computational models in our approach. In the Open state, the robot will move the end-effector to a position suitable for manipulating the door handle; autonomous fine-tuning of the end-effector pose for grasping and turning the handle is achieved by visual-servoing using the depth camera on the wrist; the motion planner executes a trajectory for appropriate action to open the door (pull or push); and finally, the robot releases the door handle, and blocks the door from closing using its other arm. In the operation of this FSM, the operator supervises the process and has the ability to intervene to repeat an action or switch to a nudge state for manual fine-tuning of the motions to precisely position the robot end-effector. The final state in the FSM is GoThrough during which the path planner generates an optimal trajectory to go through the door using a dynamic artificial potential field.

A complete progression of the Door task is depicted in Figure 4 in the simulation environment. As part of the MBD approach, validation of the algorithms designed to control Atlas in a physics-based simulation environment is critical before the algorithms can run on the actual robot. The simulation environment provides a means to test various scenarios, and techniques to complete each task. Once the human-robot system as a whole completes a task in simulation the code is tested on the physical robot. Our approach to the Door Task has proven to be effective as this is one of the tasks our team has the highest success rate (80%) to date. The challenge remains to complete the task within our time budget of 6 minutes. Our best completion time to date is 8 minutes.

## Conclusion

In our recent research and development effort that focuses on realizing effective human-supervised humanoid robot control for non-conventional emergency response, we learned essential lessons. A persistent development effort is required to complete the DRC, and this comes sometimes at the cost of perfection. For example, the DRC Trials teams created simple yet effective solutions to hard manipulation problems such as turning on a drill using a shaking behavior, or opening a door using hooks.

Providing effective and easy to use human-robot interfaces is critical. Since the DRC poses realistically challenging operating conditions for the human, the interfaces need to have suffi cient autonomy to minimize human errors. Last but not least, the factoring of the tasks between the human operator and the robot is an essential strategy to maximize the utility as a technical capability of the human-robot team. Making robots fully autonomous to complete challenging missions is still a work-in-progress.

## Acknowledgments

Authors would like to thank more than fifty students and colleagues from both WPI and CMU for their active contributions to this project. This work is sponsored by the Defense Advanced Research Projects Agency, DARPA Robotics Challenge Program under Contract No. HR0011-14-C-0011. We also acknowledge our corporate sponsors NVIDIA and Axis Communications for providing equipment support.

## References

References
1.
Mathew
DeDonato
,
Velin
Dimitrov
,
Ruixiang
Du
,
Ryan
Giovacchini
,
Kevin
Knoedler
,
Xianchao
Long
,
Felipe
Polido
,
Michael A.
Gennert
,
,
Siyuan
Feng
,
Hirotaka
Moriguchi
,
Eric
Whitman
,
X.
Xinjilefu
,
Christopher G.
Atkeson
,
Human-in-the-Loop Control of a Humanoid Robot for Disaster Response: A Report from the DARPA Robotics Challenge Trials
,
Journal of Field Robotics, Special Issue on the DARPA Robotics Challenge Trials
, vol.
32
, no.
2
, pp.
275
292
2015
.
2.
Siyuan
Feng
,
Eric
Whitman
,
X.
Xinjilefu
, and
Christopher G.
Atkeson
,
Optimization-based Full Body Control for the DARPA Robotics Challenge
,
Journal of Field Robotics, Special Issue on the DARPA Robotics Challenge Trials
, vol.
32
, no.
2
, pp.
293
312
2015
.