Perceptive whole‐body planning for multilegged robots in confined spaces

Legged robots are exceedingly versatile and have the potential to navigate complex, confined spaces due to their many degrees of freedom. As a result of the computational complexity, there exist no online planners for perceptive whole‐body locomotion of robots in tight spaces. In this paper, we present a new method for perceptive planning for multilegged robots, which generates body poses, footholds, and swing trajectories for collision avoidance. Measurements from an onboard depth camera are used to create a three‐dimensional map of the terrain around the robot. We randomly sample body poses then smooth the resulting trajectory while satisfying several constraints, such as robot kinematics and collision avoidance. Footholds and swing trajectories are computed based on the terrain, and the robot body pose is optimized to ensure stable locomotion while not colliding with the environment. Our method is designed to run online on a real robot and generate trajectories several meters long. We first tested our algorithm in several simulations with varied confined spaces using the quadrupedal robot ANYmal. We also simulated experiments with the hexapod robot Weaver to demonstrate applicability to different legged robot configurations. Then, we demonstrated our whole‐body planner in several online experiments both indoors and in realistic scenarios at an emergency rescue training facility. ANYmal, which has a nominal standing height of 80 cm and a width of 59 cm, navigated through several representative disaster areas with openings as small as 60 cm. Three‐meter trajectories were replanned with 500 ms update times.

spaces. Planning algorithms must compute both foot trajectories and body poses which ensure stability and avoid collisions with the environment. A robot may need to plan steps over obstacles on the ground while also lowering or rotating its body to pass under an overhanging obstacle, such as in Figure 1. Since prior information is not guaranteed, a robot also needs sensors to map the surrounding terrain.
The principal challenge in planning for legged robots comes from the complexity of the problem and often motivates a trade-off between planning time, detail of the robot model, and planning horizon.
Current whole-body planners for multilegged robots have fast update rates but do not consider collisions of the robot's body with terrain (Fankhauser, Bjelonic, Bellicoso, Miki, & Hutter, 2018;Magaña et al., 2019;Mastalli et al., 2017). Sampling-based planners that do consider these collisions take significant time for computation and have not been demonstrated on real robots (Grey, Ames, & Liu, 2017;Kumagai, Morisawa, Nakaoka, & Kanehiro, 2018;Short & Bandyopadhyay, 2018). Geisert, Yates, Orgen, Fernbach, and Havoutis (2019) have shown that the acyclic planner from Tonneau et al. (2018) can be applied to multilegged robots; however, their method relies on inflating the collision model of the robot, which would prevent it from working in confined spaces. Additionally, it takes several seconds to generate a trajectory and has not been demonstrated on a real robot.
In this paper, we attempt to fill the gap in current methods and enable multilegged robots to walk in confined spaces. Our method generates several meter trajectories of the 6DOF pose of the torso and plans the three-dimensional (3D) positions of the feet for each footstep. Our approach is hierarchical, first sampling a feasible trajectory of body poses, which is then smoothed using a gradient descent method similar to Covariant Hamiltonian Optimization for Motion Planning (CHOMP; Zucker et al., 2013). Next, footsteps and swing trajectories are adapted based on the terrain similar to . This way, a robot can crawl under or step over obstacles. The final phase of our planner is the optimization of the body pose to maintain the nominal rotation and height while also ensuring static stability while taking a step. Our method can update fast enough for replanning on a real robot. Our main contributions are highlighted below: • We present a method for whole-body planning for multilegged robots which generalizes to different platforms and can run onboard a real robot with less than half a second update time.
• Our method is particularly suited for confined spaces as we do not inflate the collision model of the robot. By combining sampling with trajectory optimization, we also avoid many local minima associated with confined spaces.
• Our method is one of the few to be demonstrated on a real robot and, to the best of our knowledge, to first to be deployed in field experiments. We show our robot navigating confined spaces used for training emergency rescuers.

| RELATED WORKS
While many DOF enable legged robots to traverse challenging terrain, it also makes whole-body planning computationally demanding.
Despite these challenges, there exist many examples of perceptive whole-body planning. In this section, we discuss the current state of the art and identify gaps in capabilities.

| Trajectory planning
There exists a significant body of work on trajectory planning in 3D.
Typically approaches attempt to formulate an optimization problem that balances obstacle avoidance with reaching a goal state. TrajOpt (Schulman et al., 2014) is one method which uses a Sequential Quadratic Programming (SQP) solver to compute trajectories. To check for collisions, they decompose the robot geometry and objects in the environment into meshes or convex 3D primitives and compute the closest points on each object to the other. CHOMP (Zucker et al., 2013) is another optimization-based approach which instead uses a Signed Distance Field (SDF) for collision checking. These fields are a discretization of 3D space where the value of each voxel is the distance from that voxel to the nearest object. Figure 2 shows an example from simulated data. Once the SDF is generated, it can be queried for collisions in constant time. In our previous work (Buchanan et al., 2019), we used CHOMP and SDFs to plan a robot's body height and leg span in F I G U R E 1 ANYmal autonomously navigating confined spaces using onboard mapping and no prior knowledge of the environment. The footholds, body poses, and transitions were all planned on the robot [Color figure can be viewed at wileyonlinelibrary.com] BUCHANAN ET AL.
| 69 confined spaces. We assumed, however, flat terrain and did not plan for the footholds of the robot nor the robot's 6DOF pose. The work described in this paper significantly extends the capability of our previous planner, and we show in simulations how we enable a robot to walk through previously untraversable terrain.
Other trajectory planners employ random sampling. In this case, rather than explicitly defining an optimization problem, only the constraints and state costs are defined, and robot states are randomly sampled until a valid path is found. The rapidly exploring random trees (RRT) algorithm (Lavalle, 1998) has been used extensively in this regard.
Adapted algorithms can gain some of the useful features of optimization problems, such as asymptotic optimality (Karaman & Frazzoli, 2011).
Directed methods, such as RRT-Connect (Kuffner & LaValle, 2000), can efficiently find paths between points, but these methods suffer from the curse of dimensionality, and thus, it can be challenging to plan for legged robots online. This has motivated several different approaches and compromises to planning for legged robots.

| Planning for legged robots
Many whole-body planners for multilegged robots are primarily focused on finding footholds in rough terrain and optimizing body posture for stability. Mastalli et al. (2017) and  both search in an elevation map for footholds on suitable terrain. Magaña et al. (2019) train a Convolutional Neural Network (CNN) to adapt footholds based on the elevation map for quicker reactions to disturbances. These methods have shown quadrupeds, climbing stairs, and walking in rough terrain. However, they do not consider collisions of the body of the robot with the terrain and would not be able to walk in confined spaces.
Sampling planners have been used successfully for legged robots (Grey et al., 2017;Kumagai et al., 2018;Short & Bandyopadhyay, 2018), where the focus is typically on reducing computational complexity by reducing the sampling space. Grey et al. (2017) do this by simplifying the model of the robot and reducing the space of feasible motions. Short and Bandyopadhyay (2018) save on computation by sampling a roadmap of feasible configurations for their quadruped offline in the absence of any terrain. Then, when planning for a given environment, the sampling space is significantly reduced, allowing planning in complex environments. Along similar lines, Kumagai et al. (2018) precompute a global footstep path then perform the more time-sensitive body motions online. All of these methods demonstrate complex planning through cluttered and narrow environments but are still very computationally demanding, taking several seconds to generate a plan. It is also notable that none of these methods has been demonstrated on a real robot.
The acyclic contact planner from Tonneau et al. (2018) uses a hierarchical approach and is the most similar method to our own. Similar to our method, they initially sample a trajectory of feasible poses for the base of the robot. Unlike our method, they use an inflated abstraction of the robot model to check for collisions. While this speeds up planning considerably, it is notable that this would not be suitable for planning in confined spaces. Similar to Short and Bandyopadhyay (2018), they sample from a selection of precomputed configurations, selecting configurations that avoid collisions and maximize manipulability. This step is by far the most time-consuming part of their pipeline. In contrast, our method is much faster at selecting footholds because we constrain the contact planning step to a 2D manifold. This simplification makes our method less appropriate for humanoid robots but massively speeds up planning and is quite reasonable for a multilegged robot. Geisert et al. (2019) implemented the planner from Tonneau et al. (2018) in simulation on a quadruped robot. They bias the contact sampling to encourage a suitable Support Polygon (SP), and they use an inflated collision model to prevent the robot from lowering its torso. In contrast, we enforce stability as an optimization constraint and use the exact collision model of the robot's torso.
Like the majority of the methods above, we rely on static stability. If the robot's Center of Mass (COM) lies within the convex hull of foot contacts projected onto the horizontal plane, the robot is assumed stable. As we use multilegged robots with at least four legs, we can exploit this approximation, although the robot must walk more slowly.
Other methods incorporate the robot's dynamics directly in the optimization problem. Winkler, Bellicoso, Hutter, and Buchli (2018)  This modeling enables them to plan for dynamic gaits, which increases robot speed and robustness against disturbances. They do not, however, consider collisions of the body with the terrain and must sacrifice either planning time or planning horizon.

| Mapping
For a robot to plan and walk through real confined spaces, it must be able to map the terrain and identify free space. OctoMap (Hornung, Wurm, Bennewitz, Stachniss, & Burgard, 2013) has been widely used F I G U R E 2 We show a 3 m × 3 m × 1 m SDF, centered on the robot, with two cross sections at = z 0.5 m and = y 0 m so that the robot model and obstacle are visible. The 5 cm voxels are colorized to indicate distance from obstacles. Cyan voxels are closer to the obstacle while purple voxels are further away. SDF, Signed Distance Field [Color figure can be viewed at wileyonlinelibrary.com] in robotics for occupancy mapping; however, it is not particularly efficient for collision checking as data look-ups require tree traversal.
Voxblox (Oleynikova, Taylor, Fehr, Siegwart, & Nieto, 2017) builds 3D maps using voxel hashing, which allows it to grow the map only when there is new data. They also compute the SDF directly from the depth measurements. However, they trade accuracy for real-time performance and use large (10-20 cm) resolutions. While this is acceptable for flying robots, which must keep a significant distance from obstacles, it is not suitable for ground robots in confined spaces.
Another method often used for ground robots is digital elevation mapping (Herbert, Caillas, Krotkov, Kweon, & Kanade, 1989). These are often called 2.5D maps because a grid is aligned with the robot's walking plane, where each cell records the terrain height. This representation of the environment has been highly successful for legged robots (Belter, Łabecki, Fankhauser, & Siegwart, 2016;Fankhauser, Bloesch, & Hutter, 2018) because it is very memory efficient and provides a useful approximation of the ground for foot placement. We use multi-elevation maps (Buchanan et al., 2019), which are a compromise between 2.5D and 3D mapping. We cluster depth measurements into two elevations below and above the robot.
An SDF can then be computed between these two 2.5D maps.

| POSTURE PLANNING
In this section, we will go into the details of our planning method. As shown in Figure 3, we use a hierarchical approach to planning, starting with a goal pose and current robot posture. We first randomly sample the planning space to find a feasible trajectory of discrete body poses. Due to the randomness of this method, this trajectory is highly irregular, therefore we perform smoothing in continuous time. This trajectory can be reused for online planning.
For each pose in the trajectory we seek to find four-foot contacts which are selected from the map based on terrain suitability. The body pose is optimized for stability to ensure the footholds are possible to attain, and finally leg swing trajectories are computed to avoid collisions. Table 1 summarizes information for each planning step. In the second column we highlight which parts of the robot model are considered, the third column details if, and how, collisions are checked, and the last column indicates the sections in this paper describing the planning module.

| Preliminaries
We define frame I as our fixed inertial frame of reference. The body frame B is attached to the robot's torso and moves with the robot.
A pose is described with a 4 × 4 transformation matrix belonging to the special Euclidean group SE(3). The robot's body pose relative to the inertial frame is written as T IB which is given by is the rotation matrix that projects the components of a vector from the body frame B to the inertial frame I and  ∈ t IB where N is the number of legs of the robot. Our goal is to compute P I for a legged robot from its start to a goal point while avoiding obstacles.

| Constraints
To ensure that the final plan is feasible and safe for a robot to carry out we impose several constraints in each phase of planning. In this section, we define and describe these constraints: 1. Contact Reachability: The robot must be able to attain every body pose while maintaining contact with the ground. To ensure this, F I G U R E 3 Information flow diagram. The planner receives a goal pose defined in (1) and begins to plan a trajectory for the robot. An initial trajectory of poses is sampled from an SDF which is then smoothed. This trajectory is resmoothed for replanning each footstep. Initial footholds are generated for the next pose in the trajectory so that every pose corresponds to a posture as defined in (2). Footholds and body pose are then optimized based on terrain costs and solving an NLP problem, respectively. Finally, the swing trajectory is computed. NLP, Nonlinear Programming; SDF, Signed Distance Field [Color figure can be viewed at wileyonlinelibrary.com] we set a maximum pose height parameter h max which is the maximum allowable height above the terrain elevation. 1 2. Sequential Reachability: It must be possible for the robot to transition between two sequential body poses with a single step. We enforce this by limiting the Euclidean distance between two sequential body poses to be less than a maximum step distance parameter s max .
3. Collisions: Body poses must avoid collisions with the terrain.
We 4. Stability: Postures should be statically stable. To ensure this, we consider the SP, which is defined by the convex hull of the feet positions projected onto a plane whose normal aligns with gravity.
If the COM of the robot, projected onto this plane, lies within the polygon, the posture can be said to be stable. We check for this in the posture optimization step, and details can be found in Section 3.4.
In Figure 4 we illustrate how the first three constraints can be violated. The initial pose sampler and the trajectory smoother consider constraints 1-3 from above while the stability constraint is handled by the final pose optimization which takes place after foothold selection. Since the optimization occurs in the final step, the robot's body pose may have to change to ensure stability, and this could result in a collision with the terrain. This limitation is a result of using a hierarchical planner: lower levels of planning may not be able to consider all of the constraints. In practice, we have found the few body collisions to be minor and have never interfered with the robot's mission.

| Initial pose sampling
We initially sample a trajectory of discrete body poses from the robot's state to the goal. The sample space is a 3D box which is large enough to contain the two endpoints of the trajectory with additional space in the positive and negative X and Y directions so that paths can be found around obstacles. We use the RRT-Connect algorithm (Kuffner & LaValle, 2000) which generates two RRTs, one rooted at the start and the other at the goal. Poses are randomly sampled and added to each tree using a greedy heuristic until the trees connect.
We reject poses that violate constraints 1-3 and use the SDF from the map to avoid collisions as a hard constraint.

| Trajectory smoothing
Our method for smoothing the body pose trajectory is based on the local optimization method CHOMP (Zucker et al., 2013). To use this planner, we reformulate our trajectory in continuous time as where x y z , , represent the position and ϕ θ ψ , the rotations in Euler angles of the robot body at time t. The robot's maximum absolute pitch and roll are limited to ∘ 45 which allows us to use Euler angle rotations without danger of singularities. We use the functional gradient descent update rule given by where the norm A is formed by multiplication of differencing matrices and acts as a smoothing operator. This is similar to Tonneau et al. (2018) but only considers height rather than an approximation of the workspace of each leg. Our constraint eliminates the need to generate a reachable workspace for every robot and we have found it to be sufficient for multilegged robots.
where x is the collision distance queried from the SDF and r c the collision check radius. The obstacle gradient is projected along the gradient of the SDF so that the trajectory is pushed away from all obstacles.

| Foothold planning
In the previous sections, we were concerned with planning a trajectory of body poses, here we discuss how for each pose in the trajectory, the foothold t IF I is computed. In contrast to body poses, we only plan for the next desired foothold rather than the full trajectory from start to goal pose. As the robot moves, more of the terrain will become visible and mapped. By planning a trajectory of body poses, we can look ahead and replan paths around obstacles. Since the robot moves relatively slowly, it would be unnecessary to plan more than a few footsteps, so we save on processing by planning only one step in advance.
The robot has predefined nominal feet positions,t F i . We use a fixed, statically stable gait: right-hind (RH), right-fore (RF), left-hind (LH), and left-fore (LF). With these parameters and a given body pose in the trajectory, we can compute the next ideal foothold geometrically.
We then search in the elevation map around this nominal foothold and evaluate foothold scores based on the terrain. The nominal foothold is updated with the foothold with the best score that is also kinematically feasible. The swing trajectory is initialized with knot points that are then adapted to avoid collisions. More about this process can be read in  as we use the same method.

| Pose optimization
The nominal next body poseT IB comes from the trajectory of body poses computed in Section 3.2.2. From this pose, the terrain-aware next footholds t IF I i were selected from the elevation map, and now a final optimization step is done to adjust the body pose to ensure the Stability constraint is not violated. We formulate this as an NLP problem, which we solve with an SQP solver. This method is sensitive to initialization, so to avoid falling into a local minimum, we first make a guess based on Next, we seek to minimize the following optimization objective while considering the constraints: where t IB and R IB are the final body position and rotation, respectively. Table 2 gives the equations and a description for each

| MAPPING AND COLLISION CHECKING
Our goal is to employ the posture planner online with a robot that is also mapping its surroundings. Not only do we need a method for modeling the environment and robot, but also a method to check for collisions. This section covers these concepts briefly as many of the

| Multilevel elevation mapping
The robot's map consists of a 2D grid defined centered on the robot's body which follows it as it moves. On this grid two elevation levels are defined, one which represents the terrain below the robot and is denoted floor and another for obstacles above denoted ceiling.
In Figure 5 we show how this mapping method works in practice in narrow spaces.

| Collision checking in SDFs
To plan collision-free trajectories in 3D space, we convert our elevation maps to SDFs. Each voxel in an SDF contains the signed distance from that voxel to the nearest obstacle boundary where free space has positive values and space inside obstacles are negative (see This way, we can spend less computation checking the entire surface of the robot while still having reasonable assurance of detecting all collisions. We compute these checkpoints in the robot's body frame at start-up to save on processing during run time.

F I G U R E 5
The left image shows ANYmal inspecting a narrow space and the right image is a rendering of the resulting elevation map. The red mesh represents the floor elevation and the blue mesh represents the ceiling [Color figure can be viewed at wileyonlinelibrary.com]

| IMPLEMENTATION
To perform experiments, we implemented our planner on the quadrupedal robot ANYmal . We use an Intel Realsense D435 camera at a roll angle of ∘ 90 to perceive terrain above and below the robot. Pointclouds from the Realsense are processed on a dedicated PC with Intel i7-7600U processor at 3.5 GHz. The elevation maps are encoded using the GridMap ) data structure and are passed to the second PC (which has similar specs) at 2 Hz over a wired Local Area Network (LAN) connection. Once a map is received, the SDF is computed in a separate thread using the approach presented in Felzenszwalb and Huttenlocher (2012) and saved in memory where it can be queried as needed.
Our planner starts when a new goal is received. In our experiments, the goal was always 3 m ahead of the robot. For the initial pose sampling, we use the RRT-Connect implementation in the Open Motion Planning Library (Şucan, Moll, & Kavraki, 2012). Searching is limited to 1 s, but in practice, it takes around 100 ms to find an exact solution. Table 6 in Section 7 shows the best, average, and worst-case times for each step in the planner. Once a successful pose trajectory has been found, we proceed to the smoothing step. In later planning

| EXPERIMENTS
We performed three sets of experiments. The first was a series of simulated environments which explored the versatility of the planner. The next two sets of experiments were deployed on a real robot to verify our method's applicability. Online experiments were first performed in the lab using an adjustable door and then later in the field at a training facility for emergency responders. In lab experiments, we tested some of the same environments as in simulation.
The purpose of the field experiments was to test the robustness of our method to realistic conditions.

| Simulated experiments
We created five different environments in simulation: Low Gap,

Rotated Gap, Low Gap with
Step, Thin Gap, and Random Obstacles. In each case we tested 10 trials with different dimensions or number of obstacles. Figure 7 shows the robot traversing three of these en-

vironments. It is worth noting that the Occupational Safety and
Health Administration (OSHA) in the United States defines entry portals <24 in. (61 cm) in the smallest dimension as "Restricted" since any smaller would be impossible for a rescuer to enter with breathing equipment (OSHA, 2011).

Low Gap:
In this experiment we progressively lowered the door in increments of 5 cm, each time having ANYmal pass through to the other side. We did this repeatedly until we reached a gap only 60 cm high and 1 m wide. We found the robot could pass under gaps of 70 cm and above with 100% success, 65 cm with 70% success, and 60 cm with 40%.

Rotated Gap:
In the next environment, we created an opening in the shape of a 1-m high and 1-m wide isosceles right triangle. The purpose of this was to test navigating in differently shaped confined spaces. When initially tested in simulation, we did not randomly sample the initial trajectory and used a straight-line initialization instead. We found it was rare for CHOMP to solve a trajectory even when employing the Hamiltonian Monte Carlo method as in Zucker et al. (2013). Once we introduced an entirely randomly sampled trajectory as initialization, CHOMP converged much faster to better trajectories. In the simulations, the robot could pass through this gap with 100% success in 10 trials.

Low Gap with
Step: This environment was created by lowering an overhanging obstacle to 70 cm, and raising a step in increments of 5 cm. In simulation the robot could step over 5 cm with 90% success and 60% for 10 cm. of the time, and 60 cm 20%.

Random Obstacles:
Here we created a 1 m × 1 m gap in a wall in front of the robot. We then randomly spawned 2 cm × 5 cm × 5 cm blocks in the gap which were fixed in the air. This created randomized narrow spaces which could include obstacles above or below the robot. Sometimes the robot had to crawl under, step over or rotate its torso around obstacles. We performed 10 trials with 3 blocks, 5, 7, and 9 and found success rates of 70%, 50%, 40%, and 10%, respectively.

| Indoor experiments
We performed indoor experiments using a custom adjustable doorway. The space can be made narrower by sliding the plywood door downwards along aluminum profiles. The door can also be rotated, creating an angled gap and a metal bar can be raised to create a step.  Step. In each case, we arranged the door to create the desired gap, then placed the robot in front and commanded a goal on the other side. All planning and perception were done on the robot without any prior knowledge. We recorded the robot kinematics to observe how it executed the plans.

| Low Gap
On the real robot, we found similar success rates as in simulation.
Failures were always either caused by collision with the door or singularities in computing the inverse kinematics. As the robot lowers its torso significantly, the hip joints become very close to the feet.

| Rotated Gap
In experiments with the real robot, the planner succeeded in five out of seven trials, and we show the results from one of these trials in the middle plot of Figure 8. The planner optimized a combination of roll, pitch, and yaw to orient the body through the free space. This demonstrates how the optimization keeps the COM inside the SP as a more substantial roll would be less stable. Because we include this in our objective (6), the planner instead finds an optimal combination of rotations so that none is too extreme.
The planner guided ANYmal's position to the right to avoid the narrower part of the triangle on the left. This way, once the front of ANYmal passes through the gap, large rotations are no longer needed, and the body straightens out. We also tried to reduce the angle of the doorway to make the space even more narrow, but this quickly led to higher rates of failure. The main reason for failure was the top of the robot colliding with the door. It was found that our mapping method does not work well with angled walls. There are inherently gaps in the map where floor changes to ceiling, and on an angled wall, these unmapped spaces can be significant and are located directly in the center of the door. This gap was much more pronounced in the real experiments as opposed to simulation due to higher sensor noise. The result was the planner guiding the robot very close to the door and sometimes directly into collision.

| Low Gap with Step
The final indoor experiment we performed was the Low Gap with Step.
ANYmal could step over 5 cm with 100% success which dropped to 50% at 10 cm. From the resulting plot at the bottom of Figure 8, we can see how the robot lowers its body to be below 70 cm. To overcome the higher step, the robot pitches upwards before the gap, which is the opposite behavior to the Low Gap experiment. This is because we maximize reachability allowing the front legs to swing up and over the step. Once the body has passed through the gap, the robot slowly pitches forwards and raises its body. The two spikes in pitch and roll after the doorway correspond to the two back legs stepping over the obstacle.
This lab experiment was the most challenging. The robot typically failed due to singularities, which is similar to the Low Gap experiment at 60 cm. In such a confined space, the manipulability of the legs is very low. We increased the default foot positions in the X and Y directions to increase the distance between the feet and hips, but it could not eliminate the problem.

| Weaver experiment: Rotated Gap
The robot would not have been able to walk through any of these confined spaces without the contributions outlined in this paper.
To demonstrate this, we repeated the Rotated Gap experiment in simulation on the robot Weaver . With our previous planner (Buchanan et al., 2019), the robot could not find a path and continuously collided with the obstacles. This is because it did not use an initially sampled trajectory to guide the torso, which resulted in the smoothing process diverging or becoming stuck in a local minimum. Additionally, without planning for the torso's roll and with the large collision abstraction of the whole body, it made it more difficult to find a path. We show an image from these simulations in Figure 9. It would have also been im- We visited the Training Village in July 2019, and over the course of a week, found several confined spaces to test our planner. We show here our results from ANYmal navigating three different confined spaces that firefighters could be expected to crawl through as part of their training. In each case, we placed ANYmal in front of the opening and only commanded the robot to go forwards. All perception and planning were done onboard the robot, which explored completely autonomously and untethered.

| Rectangular Gap
The first confined space we tested in the Training Village was a rectangular opening 80 cm high and 70 cm wide, which leads into the side of a burned concrete building. It also included a 10-cm step, which required ANYmal to climb up into the small space. The top of Figure 10 shows ANYmal climbing in this space. To allow ANYmal to enter, we opened a small metal door and cleared some vegetation. As ANYmal is nominally 80 cm high, it would typically barely fit inside this space. However, our planner had the robot lower its torso so that it could enter the house and map the interior.
Also in Figure 10, we show the position and rotation of the robot's base. ANYmal did not collide with the walls and was able to enter this space twice. In each case, however, the mapping failed for a part of the ceiling inside the building. The result was that the planner raised the body slightly too early and came very close to the wall. This problem is likely due to a difference in lighting between outside (bright sunlight) and inside (unlit darkness) the building. We have found that a significant contrast in lighting can negatively affect the Realsense camera.

| Crumbling Wall
The next task we attempted was to have ANYmal walk along a Crumbling Wall. As shown in the middle of Figure 10, there are several large slabs of concrete that lean against the wall creating triangular holes. The ground was flat, and the area was less confined than the Rectangular Gap; however, large pieces of concrete blocked the robot's path, which forced it to walk around the obstacles. The first obstacle on ANYmal's left was barely in the field of view of the camera, and so was not mapped very effectively, which led to the robot walking very close.
The overhanging obstacle is quite high and close to the wall, so it was not a significant issue for ANYmal to fit inside.

| Collapsed Building
The final experiment we attempted was to enter a pile of rubble created by a building collapse, as shown at the bottom of Figure 10.
This space was highly constrained due to the ledge on the robot's left and the large curved piece of concrete on the right. There was also a 5-cm depression in the floor. As shown from the odometry in Because of the very confined space, our planner initially failed to find a body trajectory until we reduced the collision radius r c from 5 to 2 cm. This change made it possible to plan trajectories, but this likely resulted in the robot scraping against the concrete. The robot was able to continue walking but, because of the statically stable gait, if the contact had been worse, the robot would have fallen over.

| RESULTS AND ANALYSIS
Our experiments represent a wide variety of narrow spaces a robot could be expected to walk through. We tested our method in five different simulated environments including with randomized obstacles. We then replicated three of these experiments in trials on the real robot. A summary of the simulation only experiments is given in Table 4, while experiments performed both in simulation and in the lab are summarized in Table 5. Success rates on the real robot matched the simulated results very closely, although the Rotated Gap had more failures on the real robot due to problems with mapping). In Table 3 we show the minimum and maximum values of the robot torso height and rotation. For height, we give the measurement of the very top of the robot's torso. Finally, we also performed three different field experiments 2 in which ANYmal was successfully able to crawl inside confined spaces at a training facility for firefighters.
We additionally recorded the timing of each step in our planner during the lab experiments. Table 6 shows the best, average, and worst time that it took for each of the planning steps to be executed.
These times were recorded on ANYmal directly as it walked through each confined space. Initial sampling and smoothing are by far the most time-consuming steps in the planning pipeline; however, timing typically improves as the robot walks since we reuse the trajectory and repeat the smoothing. Total planner time excludes SDF generation, which is computed in a separate thread. The average planning time for all experiments was <500 ms.
F I G U R E 9 (Left) The hexapod robot Weaver, (Middle) Rotated Gap experiment using our previous work, and (Right) The same environment using the methods in this paper. We did not plan footsteps for Weaver as the controller did not support this feature. In white, we show the skeleton of the robot's model and in green we show the collision model. The red mesh represents the floor elevation and the blue mesh represents the ceiling [Color figure can be viewed at wileyonlinelibrary.com] 2 A video showing ANYmal performing all of the online experiments discussed in this paper is available at https://youtu.be/C2e0JTdwid0 and is included in the online version of this article. See the appendix.  Step (5 cm) Step (10 cm) Simulation 10/10 7/10 4/10 10/10 9/10 6/10 Building. However, if we were to incorporate dynamics into our optimization, our planner would be more robust to these disturbances.

Real
This would, of course, lead to more computational requirements and require planning times fast enough to react.
A truly 3D mapping approach that does not cluster into floor and ceiling could reduce map artifacts and, therefore, lower failure rates.
This would lead to improvements in walking around sharp objects or through angled gaps, such as the Rotated Gap. Future work in this area should focus on finding the right trade-off between map detail and computation complexity.