Whole-body motion and footstep planning for humanoid robots with multi-heuristic search

Robotics and Autonomous Systems 116 (2019) 51–63

Contents lists available at ScienceDirect

Robotics and Autonomous Systems journal homepage: www.elsevier.com/locate/robot

Whole-body motion and footstep planning for humanoid robots with multi-heuristic search ∗

Rizwan Asif a,d , , Ali Athar b , Faisal Mehmood a , Fahad Islam c , Yasar Ayaz a ,1 a

RISE Lab, SMME, NUST Main Campus, Sector H-12, Islamabad, Pakistan Technical University of Munich, Arcisstrasse 21, 80333 Munich, Germany c The Robotics Institute, 5000 Forbes Avenue, Pittsburgh PA, USA d EIT ICT Labs, KTH Royal Institute of Technology, SE-100 44, Stockholm, Sweden b

highlights • • • •

Humanoid motion planning is challenging due to search space complexity and constraints. A motion planning framework based on a greedy graph search algorithm is proposed. Proposed framework uses multiple heuristics and motion primitives to guide the search. Experimental results for planning challenging tasks are presented and justified.

article

info

Article history: Received 12 August 2018 Received in revised form 17 March 2019 Accepted 18 March 2019 Available online 21 March 2019 MSC: 68-40 Keywords: Robotic motion planning Humanoid robots Multi-heuristic A* Graph search

a b s t r a c t In this paper, we present a motion planning framework for humanoid robots that combines wholebody motions as well as footsteps under a quasi-static flat ground plane assumption. Traditionally, these two have been treated as separate research domains. One of the major challenges behind wholebody motion planning is the high DoF (Degrees of Freedom) nature of the problem, in addition to strict constraints on obstacle avoidance and stability. On the other hand footstep planning on its own is a comparatively simpler problem due to the low DoF search space, but coalescing it into a larger framework that includes whole-body motion planning adds further complexity in reaching a solution within a suitable time frame that satisfies all the constraints. In this work, we treat motion planning as a graph search problem, and employ Shared Multi-heuristic A* (SMHA*) to generate efficient, stable and collision-free motion plans given only the starting state of the robot and the desired end-effector pose. © 2019 Published by Elsevier B.V.

1. Introduction The task of computing motion plans for humanoid robots to perform various tasks presents multiple challenges. In addition to being collision-free and stable, these motion plans should also have some theoretical guarantee of optimality according to some performance metric. Satisfying all these constraints is fundamentally challenging due to the many degrees of freedom (DoF) of the search space, and the presence of obstacles in the environment that have to be circumvented through complex ∗ Correspondence to: RISE Laboratory, School of Mechanical and Manufacturing Engineering, National University of Sciences and Technology, Islamabad, Pakistan. E-mail addresses: [email protected] (R. Asif), [email protected] (A. Athar), [email protected] (F. Mehmood), [email protected] (F. Islam), [email protected] (Y. Ayaz). 1 RISE Laboratory, School of Mechanical and Manufacturing Engineering, National University of Sciences and Technology, Islamabad, Pakistan. https://doi.org/10.1016/j.robot.2019.03.007 0921-8890/© 2019 Published by Elsevier B.V.

whole-body movements. Furthermore, coalescing motion planning with footstep planning adds a further layer of complexity since both processes need to be merged into a single, unified framework that allows footsteps and whole-body motions to execute simultaneously. Research in the area of high DoF robotic motion planning generally involves either one or both of the following tactics: (1) applying simplifying assumptions on the environment or the robot geometry to reduce the problem complexity, and (2) formulating the planning problem in a way that existing algorithms are able to solve easily. An early approach proposed by Yoshida et al. [1] modeled a humanoid robot with a bounding box for which collision free paths were planned, and then converted to valid locomotion trajectories. However, the crude approximation technique prevents this approach from working well around obstacles. Kanoun et al. [2] planned local motions that minimized certain task functions through a numerical IK solver with taskspecific Jacobians. This approach, however, is prone to fail due to local minima as the complexity of the planning task increases.

52

R. Asif, A. Athar, F. Mehmood et al. / Robotics and Autonomous Systems 116 (2019) 51–63

In [3], humanoid footstep planning was performed by applying continuous deformation on a motion model. Formulating the latter as a virtual kinematic tree allowed the deformation to be formulated as an inverse kinematics problem. Pettre et al. [4] plan locomotion trajectories for digital actors by first planning a walking path in two-dimensional space, then converting it to a trajectory for the lower body, and then finally using a warping technique to compute a collision-free upper body motion plan. Among more contemporary works, Cognetti et al. [5] used Center of Mass (CoM) primitives to plan whole-body motions along with footsteps for humanoid robots. Their approach involved generating joint trajectories to realize each CoM primitive in the planned path, and then concatenating the trajectories in a suitable way. The drawback of this approach is that the task itself has to be defined in terms of a trajectory for a specific point on the body of the robot, and cannot simply be defined as a target pose for the end-effector. Another approach to motion planning involves treating the task as an optimization problem [6–8]. CHOMP [9] proposed a set of ideas on how numerical optimization of robotic motion planning problems should be performed. It uses pre-computed, signed distance fields for collision checking, and pre-conditioned gradient descent for the actual optimization. Another work, STOMP (Stochastic Trajectory Optimization for Motion Planning) [10], uses a gradient-free, stochastic scheme for optimization. More recently, TrajOpt [11] implements a sequential convex optimization procedure with an efficient formulation of collision avoidance constraints to compute motion plans for mobile manipulation platforms for performing complex tasks. These frameworks have yielded appreciable results, but the drawback is that optimization based approaches require an initial trajectory which can then be optimized. Another set of works tackle robotic motion planning by utilizing sampling-based algorithms such as Rapidly-exploring Random Tree (RRT) and its many variants [12,13]. These algorithms are able to quickly explore high dimensional search spaces due to the probabilistic sampling strategy involved. Perrin et al. [14] proposed a humanoid footstep planning framework which accounted for 3D environment obstacles. This was achieved by first performing offline swept volume approximations to reduce time spent on collision checking, followed by half-step planning using a variant of RRT. In [15], this approach was extended to allow the entire continuous space of actions to be used, as opposed to a discrete set. Teja et al. [16] proposed a whole-body motion planning framework that uses RRT* to generate asymptotically optimal motion plans along with a novel scheme to handle loop closure constraints in double support. Baltes et al. [17] developed an RRT-based motion planner that employs incremental simulation to account for dynamics in the motion. King et a. [18] employ an RRT based planner to perform rearrangement planning by combining robot-centric and object-centric actions. Ferrari et al. [19] perform humanoid motion planning for locomotion and manipulation tasks by using center of mass primitives, and by using three operation zones to synchronize locomotion and manipulation. Burget et al. [20] introduced Extended RRT Connect to plan collision-free whole-body motions for humanoid robots by incorporating kinematic and task constraints into the algorithm. Their approach implemented a bi-frontal search by extending the tree from both start and goal configurations. There are, however some limitations to the sampling-based approaches. Firstly because of the inherent randomization, the resulting motion plans generated for two very similar (yet not identical) queries can be drastically different. Secondly, the generated motion plans often involve superfluous motions that look unnatural, thus requiring additional smoothing steps that might be challenging in cluttered environments. Thirdly, it is challenging to ensure the optimality of the solution in finite time.

To mitigate some of these drawbacks, the family of searchbased algorithms have recently come into limelight in the context of motion planning. Even though these algorithms have been commonly used for footstep planning for quite some time [21,22], they were considered unsuitable for high DoF motion planning due to the impractically large time required to explore the search space. However, using a combination of greedy heuristics to guide the search, and motion primitives to model state transitions, it has been shown that search-based approaches can be used to do effective motion planning for robotic arms [23,24] and mobile manipulation platforms [25]. In a previous work [26], we used Weighted A* (WA*) in conjunction with a floating-arm heuristic and carefully crafted yet generalized motion primitives to plan whole-body motions for humanoids with static foot positions. Recently, progress has also been made on employing search-based algorithms to compute motion plans that combine footsteps and whole-body motion plans [27,28]. The key challenge here is to come up with a robust framework that is not domain-specific and can be employed in a variety of situations. A logical way of accomplishing this is to somehow break down the complexity of the planning task into manageable portions. To this end, Dornbush et al. [28] employ adaptive dimensionality and decompose a given task into a sequence of smaller tasks that can be independently tackled. In the same spirit, we seek inspiration from a rather old work that is unrelated to motion planning; Chakrabarti et al. [29] proposed the use of islands or sub-goals to improve the performance of the A* algorithm, and Dillenberg [30] reiterated the utility of this idea in improving the efficiency of heuristic search. In this paper, we present a motion planning framework that incorporates both whole-body motion and footstep planning. At the core, we employ SMHA* [25], a multi-heuristic search framework that guarantees completeness and suboptimality bounds. We use SMHA* with diverse motion primitives and multiple heuristics. To this end, we propose a novel island heuristic that facilitates the planning process by defining lower-dimensional sub-goals that are easier to reach, and from where the final goal is accessible. Islam et al. [27] also incorporated island heuristics in their framework, but it differs from our implementation in two ways: (1) they seek human guidance to discover a suitable intermediate configuration only when the planner seizes to make progress; by contrast, we use an automatically computed island state that is always present from the beginning of the planning phase; (2) the intermediate configuration in [27] has full DoF whereas the island state proposed in this paper has a lower DoF than the planning domain. Through the aforementioned combination of heuristics and motion primitives, our framework is able to generate stable, collision-free and bounded-suboptimal motion plans in which footsteps and whole-body motions are jointly executed. This paper is organized as follows: The following section explains how the motion planning problem is framed as a graph search. Sections 3 and 4 explain the heuristics and motion primitives employed, respectively. The implementation details and experimental results of our framework are presented in Sections 5 and 6 respectively, and are analyzed and justified in Section 7. Finally, Section 8 concludes the paper with a brief summary. 2. Motion planning through graph search Generally, a graph G is a collection of vertices S and edges E, i.e. G = {S , E }. In the context of robotic motion planning, vertices denote the kinematic state of the robot, whereas edges define feasible transitions from one state to another. To this end, the kinematic state of a humanoid robot at a specific time instance can be fully described by the set of its joint offsets, and the pose

R. Asif, A. Athar, F. Mehmood et al. / Robotics and Autonomous Systems 116 (2019) 51–63

of the robot’s base frame with respect to some global reference frame. For a humanoid robot with n active joints, the complete joint state Θ can be fully described by Θ = {θi | i = 1, . . . , n} ∈ Rn , where θi is the angular offset of the ith joint. In order to describe the pose of the robot, it is assumed that the humanoid is operating on a flat, level surface, and that the base frame Fbase is the support foot. Hence, the pose of Fbase with respect to the global reference Fref is ref Tbase = [x, y, γ ] ∈ SO(2). Here, x and y denote the offsets along the x and y-axes respectively, and γ denotes the orientation of the robot on the ground (i.e. rotation around the z-axis). For the sake of brevity, ref Tbase will be denoted by Ψ from here onward. The complete static kinematic state of the robot, s ∈ S, can thus be described by concatenating the joint offsets and the robot pose, i.e. s = [Θ ⊕ Ψ ] ∈ Rn × SO(2) (⊕ denotes concatenation). In addition, an auxiliary binary variable ωs ∈ {l, r } is associated with each state s that defines which of the two robot’s feet (left or right) can be swung to execute a footstep. The value of this variable alternates in order to facilitate footstep planning. The set of edges E in the graph allow states to transition to and from one another. From here onward, edges will be referred to as motion primitives, and these are normally atomic movements that are applied to one or more joints, and possibly also the robot pose. In general, a motion primitive M can be defined by M = [∆Θ ⊕ ∆Ψ ]. It is worth noting that in practice, a motion primitive does not fully have n + 3 DoFs. This is because certain joint movements may alter the robot’s pose, and conversely, changing a humanoid robot’s pose necessitates certain leg joint movements. Motion primitives are discussed in detail in Section 4, but for now this general description will suffice. An important implication of motion primitives is that they serve to discretize the search space. To this end, the discretization can be made arbitrarily fine by using infinitesimally small motion primitives; however, this will increase the graph’s complexity and the computation time required to solve the problem. Having defined both graph states and motion primitives, it can be stated that when a motion primitive M is applied to a state s, a new state s′ is generated which is a successor of s, i.e. s′ = s + M ∈ successors(s). As previously mentioned, S denotes the set of possible states, but in practice, only a small subset of these states is valid. Let Svalid ⊂ S be the set of valid states; then, s ∈ Svalid if and only if it fulfills all of the following criteria: 1. All the joint offsets in state s must respect the physical joint limits of the robot. 2. The state s must be statically stable. This is checked by determining if the projection of the robot’s center of mass (CoM) on the ground plane lies within the convex hull of the robot’s support polygon. 3. The state s must be collision-free, both in terms of selfcollisions and collisions with obstacles in the environment. Since the search space is extremely large, it is not feasible to compute and store all possible s ∈ Svalid in advance. Instead, the successors of a particular state are generated at run-time when the algorithm decides to expand that state. The task of generating efficient, feasible motion plans for a robot can be interpreted as finding a minimum cost path from the initial state sstart to the goal state sgoal . It is worth noting that in our case, sgoal is not initially known. Rather, our framework accepts the desired end-effector pose ref Tgoal as the goal. As the dynamic graph search proceeds, forward kinematics is applied on every newly generated state to compute the end-effector pose ref Teef . If the end-effector pose is equal to the goal pose within some acceptable error margin, the search is said to have successfully completed.

53

In order to find a minimum cost path, we employ SMHA [25]. This algorithm is an extension of the well-known A* algorithm [31]. The primary difference is that whereas the former relies solely on a single admissible heuristic function (i.e. this heuristic function underestimates the goal distance) to guide the search, SMHA* allows for multiple heuristics to be used to explore different parts of the search space. A single admissible ‘anchor heuristic’ guarantees completeness and bounded sub-optimality, but the other heuristic functions are allowed to be inadmissible. This leads to faster convergence times since a combination of sensibly chosen heuristic functions can avoid local minima better than a single function. 3. Heuristics In order to plan least-cost paths from start to goal in a high dimensional space within a reasonable time frame, the algorithm is heavily dependent on informative heuristics to efficiently guide the search. To ensure completeness, SMHA* requires at least one consistent and admissible heuristic function ho , called the anchor heuristic. An admissible heuristic is one which never overestimates the true cost of reaching the goal. Aside from this, the algorithm accepts n additional heuristic functions hi | i = 1, . . . , n which need not be admissible. For any state s, the cost function associated with heuristic hi (s) is defined as Ci (s) = g(s) + ϵ hi (s), where g(s) is the cost of reaching state s from start state sstart . In our framework g(s) is the Euclidean distance between the endeffector position at state s and the starting state. As it will be seen in Section 3.1, this metric is consistent with the anchor heuristic. Essentially, g(s) allots edge costs to the portion of the graph that has already been explored, while the heuristic guides the algorithm towards the goal. The search is made greedy by tuning the weight ϵ associated with the heuristic. Furthermore, ϵ also serves as the suboptimality bound for the solution. We employ a variant of Multi-Heuristic A*, namely Shared Multi-Heuristic A* (SMHA*), in which the set of currently explored states are shared among all heuristics. Hence, a promising state discovered by one heuristic can be further explored by other heuristics, and these heuristics can collaboratively expand the search in the right direction while avoiding local minima. In our framework, we use a combination of three dimensional and two dimensional heuristics which are explained in the following sub-sections. 3.1. Breadth-first search 3D Similar to previous works [23,24,26], we use 3D Dijkstra’s search to determine the heuristic cost since it allows the end effector to circumvent obstacles present in the environment. 3D Dijkstra’s search or Breadth First Search (BFS 3D) models the end effector as a floating hand that is effectively detached from the body and hence, freely movable in space. This means that the heuristic always underestimates the true distance to the goal, and is therefore admissible. In our framework, this is the anchor heuristic. To implement this, a 3D grid (sometimes referred to as an occupancy grid) is constructed in which the environment is discretized into voxels. This occupancy grid is then populated with environment obstacles by tagging those voxels which correspond to obstacle locations. These obstacles are subsequently inflated by the approximate radius of the gripper to take into account the size of the end effector. Thereafter, the goal voxel which corresponds to the goal position is set, and 3D Dijkstra’s search is run on the entire grid to determine the distance to the goal from each voxel. Voxels that are occupied by obstacles are arbitrarily

54

R. Asif, A. Athar, F. Mehmood et al. / Robotics and Autonomous Systems 116 (2019) 51–63

Fig. 1. Visualization of the heuristic cost function for all four BFS 2D heuristics. The images essentially present a top-down view of an environment. The dark gray boxes depict obstacles, the color denotes the heuristic cost with blue and red representing low and high costs, respectively. The goal projection is marked with a white circle (as discussed, in order to compute the heuristic cost, the goal is shifted to the nearest non-obstacle point). (a) BFS 2D heuristic without obstacle inflation, (b) BFS 2D heuristic with obstacles inflated, (c) island heuristic for the left foot, (d) island heuristic for the right foot. For the island heuristics, it can be seen that the point of minimum cost lies slightly away from the goal, and at different positions in both heuristics. These locations correspond to the left and right foot positions, respectively, from where the actual goal is reachable . (For interpretation of the references to color in this figure legend, the reader is referred to the web version of this article.)

footstep motions that direct the robot towards the goal. In order to generate the occupancy grid for this heuristic, the environment obstacles and goal pose are projected onto the floor. In case the goal pose projection happens to lie inside an obstacle projection, the 2D goal is shifted to the nearest non-obstacle cell in the grid. Then, similar to the BFS 3D heuristic, Dijkstra’s search is run on the grid and the distance from every cell to the goal cell is computed and stored. In order to compute the heuristic cost for any state, the (x,y) coordinates of the robot’s mid-feet position are mapped onto grid coordinates, and the corresponding distance value in the grid is used as the heuristic cost. One modification is that instead of having a single goal point where the heuristic cost is zero, we actually define a small radius around the goal, and the cost is zero for all cells inside the radius. This helps to prevent a scenario where the robot body actually walks until it is standing directly on top of the goal position, thereby making it unreachable. This approach, however, suffers from a drawback: modeling the 2D projection of the robot as a single point is a coarse approximation. To ensure that this heuristic function does not get stuck in local minima due to collisions, the obstacles would have to be significantly inflated to take into account the possible spread of the robot’s body. However, this would render the heuristic ineffective when trying to plan a path through closely placed obstacles. To mitigate this problem, we employ two variants of this heuristic in our framework: one in which the obstacles are not inflated, and another in which the obstacles are inflated by an amount that is roughly equal to the robot’s average spread along the x and y axes. The motivation behind doing this is to achieve a best-of-both-worlds scenario, at the cost of increased computational overhead resulting from additional heuristics. The uninflated heuristic helps discover walking paths through closely placed obstacles, whereas the heuristic in which obstacles are inflated serves as an alternative in cases where the former is stuck in a local minimum due to collisions. A visualization of both variants of the heuristic is provided in Fig. 1(a) and (b). 3.3. Island heuristic

assigned very high distance values. These voxel distances effectively serve as the heuristic cost in the following way: every point PW = {x, y, z }world in the robot’s workspace can be mapped onto a corresponding set of grid coordinates PG = {x, y, z }grid using a discretization function. The voxel distance associated with these grid coordinates is converted to appropriate units and used as a heuristic cost for the corresponding cartesian coordinates PW . It should be noted that Dijkstra’s search is only required to be performed once at the start of planning; thereafter, the entire occupancy grid is populated with voxel distances and the heuristic cost for any state can then be obtained using a constant-time lookup function. 3.2. Breadth-first search 2D A key contribution of our work is the ability of the planner to generate suitable walking motions if the goal is not within grasping range. To this end, we employ a two-dimensional version of the Dijkstra grid search explained above. While it may seem that the BFS 3D heuristic should be sufficient for this task, in practice it is highly infeasible since the large edge cost associated with a footstep motion means that the BFS 3D heuristic is reluctant to explore footsteps, and is instead more inclined towards endlessly trying small joint movements (i.e. a local minimum is encountered). The 2D variant serves as a coarse approximation of the environment in which only footstep motions result in a visible heuristic cost reduction, and is therefore helpful in the initial phase of a long-distance planning problem by rapidly generating

If the goal pose is in a cluttered environment, and is situated far from the robot’s starting pose, then in most cases the goal will only be reachable if the robot adopts a certain approach path. We propose a unique type of heuristic to tackle these situations. The underlying idea is to provide an initial, low-dimensional subgoal to guide the high-dimensional search in the right direction. The inspiration for this technique comes from an earlier work by Chakrabarti et al. [29]. This sub-goal, or island, is generated before the actual planning process starts. An imaginary circle is first drawn around the projection of the goal pose on the floor, and a set of equally spaced points are chosen on the circumference of this circle. Then, for each circumference point P, it is assumed that the robot is standing with its mid-feet position at this point (collisions are ignored at this stage). Next, an Inverse Kinematics (IK) solver is applied to compute the joint offsets that would enable the robot’s end-effector to reach the goal. This process is repeated for all points on the circumference of the circle. The resulting joint configurations returned by the IK solver are checked for collisions, and all unsuitable configurations are subsequently discarded. Finally, from the remaining configurations, the most stable one is chosen, and the circle circumference point (denoted by Pisland ) corresponding to this configuration serves as the island. In a nutshell, this point defines a feasible foot position from where the goal is reachable. A visualization of the pose sampling procedure is given in Fig. 2. In order to formulate this as a heuristic, two separate BFS 2D occupancy grids are created. The goal positions in these grids

R. Asif, A. Athar, F. Mehmood et al. / Robotics and Autonomous Systems 116 (2019) 51–63

55

Fig. 2. A visualization of the process of island generation.

correspond to the respective left and right foot positions required for the mid-feet point to be at Pisland . Suppose that for a given state s, the left and right feet are at positions (xl , yl ) and (xr , yr ), respectively. Furthermore, the heuristic distance for each of these points as looked up from the aforementioned BFS 2D grids are hl (s) and hr (s), respectively. Then, the heuristic distance that is actually provided to the planning algorithm is given by:

{ hi (s) =

hl (s) hr (s)

ωs = l ωs = r

(1)

where ωs is the auxiliary binary variable described in Section 2 which defines the swing foot for any possible footsteps that can be executed on state s. The island heuristic is thus composed of two separate heuristics that independently guide each of the robot’s feet towards a feasible position from where the goal is reachable. For any particular state, only one of these heuristic distances are used, depending on which foot is active for that state. The only external parameter governing this heuristic is the radius of the circle that was drawn around the goal, which is independent of the environment, and depends only on the armlength of the robot. Therefore, this heuristic generalizes well to a variety of different tasks. These heuristics play an important role in avoiding local minima near the goal position, especially when the goal is in a cluttered environment. A visualization of both heuristics is provided in Fig. 1(c) and (d). 4. Motion primitives Our framework utilizes a combination of different types of motion primitives. These are atomic movements that effect one or more of the robot’s joint offsets. From the perspective of graph search, motion primitives are edges in the graph with an associated cost that reflects the penalty of performing that motion. 4.1. Joint primitives These are the most general and simplest type of motion primitives, and involve a small, fixed offset to one or more of the robot’s joints. For arm joints, such movements can be made regardless of the position of other joints as long as there is no self-collision, but for leg joints, there is an additional constraint: a naive movement in one or more leg joints will almost always cause the robot to lose its balance, since both feet will no longer lie flatly on the ground. To mitigate this problem, we apply inverse kinematics on the static leg so as to keep the foot pose constant. As an example, if a certain joint primitive alters one or more joints in the right leg, then the joints in the left leg are adjusted such that the feet are still in the same position as before.

Fig. 3. A visualization of the dynamic primitive which makes the robot sit or stand. A large value of ∆z was used for this simulation in order to clearly highlight the resulting motion.

4.2. Dynamic primitives Humanoid robots require highly coordinated joint movements to display human-like motion. In particular, standing and sitting are tasks that are essential in allowing humanoid robots to perform physical tasks, but which require a systematic and simultaneous adjustment of the hip, knee and ankle joints. Our observation is that the planner is unable to derive such complex motions by concatenating simple joint primitives. Similar to our previous work [26], the current framework also employs dynamic primitives that enable the robot to stand and sit. The joint offsets required to achieve this are derived in the following way: given the current pose of the end effector (x, y, z , α, β, γ ), an IK solver is used to compute the joint offsets required to achieve a new end effector pose (x, y, z + ∆z , α, β, γ ), with an additional constraint that the arm joints are not allowed to move (this is to ensure that the IK solver does not return a trivial solution simply by moving the shoulder or elbow upwards/downwards). Depending on whether ∆z is negative or positive, the resulting motion will either cause the robot to sit or stand, respectively. An example such a motion is illustrated in Fig. 3. 4.3. Footstep primitives Similar to sitting and standing, footsteps are another type of motion that require calculated, systematic movement of multiple leg joints, and therefore require a special type of primitive in order to be executed. These footstep primitives are defined in terms of an offset to the position and orientation of either of the robot’s feet i.e. M = (∆x, ∆y, ∆γ ). For a given state s, the framework decides which foot to move based on the variable ωs described in Section 2. To facilitate footstep planning, this variable is inverted each time a footstep is executed. As an example, if a successor state s′ ∈ successors(s) was generated by applying a footstep primitive, then ωs′ = ∼ ωs . One could argue that it would be more appealing to allow the algorithm to decide for itself which foot needs to be moved. In our experiments however, we observed that doing so does not actually improve the flexibility of the framework, and only increases the planning time significantly since calculating a footstep trajectory is computationally expensive. Furthermore, there is an increased

56

R. Asif, A. Athar, F. Mehmood et al. / Robotics and Autonomous Systems 116 (2019) 51–63

likelihood of one or more heuristics encountering local minima in case the same foot is moved twice consecutively, resulting in an awkward posture from which no further footsteps are possible. In order to compute the joint trajectory for a footstep, we developed a walking gait pattern generator that accepts input in the same form as the footstep primitive definition above (in addition to which foot the primitive is to be applied on), and outputs a statically stable joint trajectory that achieves the required movement. This pattern generator is discussed in detail in Appendix B. It should be noted that footstep primitives differ from joint and dynamic primitives in that they actually consist of the complete joint trajectory required to execute the entire footstep, rather than just a single joint state. The planner is agnostic to this trajectory, however, and can only see the final, resulting joint configuration at the end of the footstep trajectory. 4.4. Adaptive primitives As mentioned previously in Section 2, the resolution of motion primitives involves a trade-off: a finer granularity ensures better coverage of the search space, but also increases the search time exponentially. Therefore, a moderately coarse resolution is used in practice. This, however, carries the additional risk that the planner may overshoot the goal and endlessly oscillate in its vicinity without achieving the accuracy margin required for task completion. To circumvent this problem, [24] proposed adaptive primitives, wherein after every state expansion, the euclidean distance between the end effector and the goal is calculated. If this distance is below a certain threshold, an IK solver is used to compute the joint offsets required to reach the goal. Thereafter, linear interpolation is used to compute a smooth trajectory from the current state to the goal. If such a trajectory is valid and collision-free, then the goal state is generated as a successor and the planning completes successfully. We employ a similar strategy in both our previous [26] and current framework, but with a key difference. When planning motions for robotics arms or wheeled manipulation platforms, if the aforementioned interpolated trajectory is in collision, a simple remedy is to re-attempt IK using a different, randomly generated seed state, but for humanoid robots, only a very limited subset Svalid of the search space S satisfies the validity criteria, so a randomly generated joint configuration will almost always be invalid. Therefore, we instead re-attempt IK using another promising state. The methodology used to generate and choose these promising states is elaborated in Appendix A. 5. Implementation details There are two parts to our motion planning framework: the planner and the environment. The planner, which includes the MHA* algorithm, is implemented using the Search Based Planning Library (SBPL) [32]. The environment on the other hand, is implemented using the MoveIt! framework [33], and includes forward kinematics and robot state and obstacle modeling. Collision checking is applied using the Fast Collision Library (FCL) [34] which uses mesh models of the robot’s links, as described in the Universal Robot Description File (URDF). The collective framework which consists of both planner and environment is made to run on the ROS platform [33]. For whole body IK, we employ the Kinematics and Dynamics Library (KDL) [35] since it provides a configurable interface for determining IK solutions to kinematic trees. For applying adaptive primitives, a 5 DoF IK solution is obtained with the grasp axis of the end-effector being unconstrained. Concerning the 3D Dijkstra’s search heuristic, we use an occupancy grid with a resolution (voxel dimension) of 5 mm, and

Fig. 4. An illustration showing the joint structure of the Nao humanoid robot. The joints labeled in red form the planning domain for the tasks in Section 6.1 . (For interpretation of the references to color in this figure legend, the reader is referred to the web version of this article.) Source: http://doc.aldebaran.com/2-1/family/nao_dcm/actuator_sensor_names. html.

model the end effector using a sphere of radius 3 cm. The occupancy grids used by the 2D heuristics also have a resolution of 5 mm. The ground projections of the obstacles in the environment were inflated by 5 cm for the second variant of the Breadth-first Search 2D heuristic. We tested our framework on a Nao humanoid robot whose joint structure is illustrated in Fig. 4. This robot has a height of 58 cm from the ground, and its DoFs are as follows: 2 in the neck, 5 in each arm (excluding the joint which opens and closes the gripper), 6 in each leg (this includes one mimic joint, namely the HipYawPitch joint as in Fig. 4 which is common to both legs and cannot be independently set). When running the graph search, we employ lazy expansion, i.e. collision checking is not performed when a new state is generated, but rather only when it is expanded. This is helpful in reducing planning time since collision checking, which is computationally expensive, is not performed for states which are never expanded. All experiments were conducted on a laptop powered by an Intel Corei5 3337U CPU with 6 GB RAM. The overall work-flow of the framework is as follows: prior to commencing the graph search, the island foot poses are computed as described in Section 3.3. After that, the distance grids for all the heuristics are initialized with the goal pose and environment obstacles. Thereafter, the SMHA* algorithm commences searching until either the goal is reached, or a timeout expires. Recall that the search proceeds by dynamically expanding the graph by applying motion primitives on graph states which the algorithm deems promising according to their true cost and heuristic value. A pseudo code of this work-flow is provided in Pseudocode. 1. Note that this is a high-level description that deals with SMHA* in an abstract manner. Readers interested in the exact algorithmic details should refer to Alg. 1 in [36]. 6. Experimental results We performed two separate experiments for testing the usability of this approach. In both experiments, the goal is defined as a 5-DoF target pose for the end-effector, with the grasp axis being unconstrained. 6.1. Planning with constant foot position In the first experiment, a simplified version of this framework, as discussed in our previous work [26], is used to perform a

R. Asif, A. Athar, F. Mehmood et al. / Robotics and Autonomous Systems 116 (2019) 51–63

57

Fig. 5. Images of the execution of motion plans for the four tasks with constant foot positions described in Section 6.1.

Pseudocode 1 Planning Process 1: Inputs: Initial joint state and foot pose, end-effector goal pose, environment obstacles. 2: Output: Statically stable motion plan to reach goal. 3: 4: Compute island positions for left and right foot. 5: Initialize heuristic occupancy grids (BFS3D, BFS2D, Island) 6: Start SMHA* search 7: while goal not reached do 8: Continue SMHA* search 9: if end effector distance to goal < threshold then 10: Augment state successors with adaptive primitives 11: Backtrace goal state to start state to get complete motion plan

set of tasks, and the results are compared to a state-of-the-art sampling-based planner operating under similar conditions. This simplified framework is equivalent to the current framework with the following modifications:

• Only the breadth-first search 3D heuristic is used. From an algorithmic point-of-view, this is equivalent to using Weighted A* [31] instead of SMHA*. • Footstep primitives are not applied, i.e., the robot maintains a constant foot position throughout the motion. • The planning domain consists of fewer joints, as illustrated in Fig. 4. Joint primitives are only applied to the 10 joints labeled in red. The left leg joints (labeled in blue) are only

adjusted using IK such that a constant foot position is maintained. Listed below is a description of the four tasks performed: 1. Slit task: The robot is grasping a small cylindrical object which it has to drop inside a cup. The arm is initially raised above the robot’s head, and the path between start and goal is blocked by a sideways wall with a narrow slit cut into it. In order to reach the cup, the robot needs to navigate its arm through the slit which is only about 1 cm wider than the widest portion of the wrist. 2. Table task: A table is positioned close to the robot and the arm is initially just above the table top. The goal is maneuver the arm between the table and the robot’s body in order to reach a point below the table. 3. Shelf task: This is a two part planning problem. In the first part, the robot starts with its arms hanging sideways, and is tasked with reaching a cylindrical object located in the top shelf with its hand opened. The hand subsequently closes to grasp the object and the first part is complete. In the second part, the robot is tasked with placing the cylindrical object in the lower shelf. It should be noted that the object’s pick and place points have obstacles placed nearby, making the task further challenging. 4. Hole task: In this task, the robot is initially in a sitting position with its arms hanging sideways. A wall facing the

58

R. Asif, A. Athar, F. Mehmood et al. / Robotics and Autonomous Systems 116 (2019) 51–63 Table 1 Experiment results for planning tasks with constant foot position: Extended RRT-Connect vs. Weighted A*. Statistic

Slit

Table

Shelf

Hole

9.01 ± 3.41 16.13 ± 3.58 212 ± 89 85.3

12.42 ± 7.30 17.36 ± 4.97 573 ± 240 86.8

46.25 ± 13.43 42.73 ± 5.45 2147 ± 408 61.8

33.36 ± 12.11 25.80 ± 3.38 1745 ± 653 53.0

17.93 ± 1.72 11.24 ± 1.79 75.4 ± 4.14 85.1

31.94.37 ± 2.39 10.18 ± 1.31 142.9 ± 5.34 77.2

17.34 ± 1.84 19.97 ± 2.01 63.08 ± 5.92 68.9

12.92 ± 1.07 8.10 ± 0.83 37.4 ± 2.06 78.7

Extended RRT-Connect [20] Planning time ± std. [s] Path length ± std. [rad] Number of nodes ± std. Success rate [%] Weighted A* (ours) Planning time ± std. [s] Path length ± std. [rad] State expansions ± std. Success rate [%]

Table 2 Experimental results for the planning tasks with walking motion. The path length measures the absolute sum of the joint offsets which all the robot’s joints underwent for the motion plan’s execution. The total cost corresponds to the g-cost assigned to the goal state, and is a measure of the total distance moved by the end-effector. Statistic

Shelf 1

Shelf 2

Table

Number of state expansions Path length [rad] Total cost [m]

1779 271 41.79

678 446 19.26

1474 993 36.84

Pre-processing time [s] Collision checking time [s] IK computation time [s] Total planning time [s]

3.5 301.2 85.3 477.8

3.7 99.8 28.6 159.3

3.4 280.0 69.1 469.8

robot is placed less than 10 cm in front of the robot’s feet and the goal is to reach a point that lies 6 cm behind the wall. In order to reach this location, the robot must stand up and maneuver its arm through a 9 cm × 9 cm hole in the wall. For each task, we sampled 100 different goal poses with (x, y, z) coordinates located in a spherical region of interest with a diameter of 4cm. The goal orientation was defined in terms of a 15◦ range for the roll, pitch and yaw angles. Poses were then sampled uniformly under these settings, and IK was applied on each pose (neglecting collisions) to ensure that it was actually reachable. Then, our planning framework was applied to each pose to compute a valid motion plan. Note that the spherical region from which goal positions were sampled was defined such that a floating end-effector could reach the goal without colliding. The generated motion plans for all four tasks for a given goal pose can be visualized in Fig. 5.2 In order to highlight the efficacy of our framework, we performed the same tasks with the same set of goal poses using one of the state-of-the-art humanoid motion planning approaches [20]. It employs a variant of the Rapidly-exploring Random Trees (RRT) algorithm, namely Extended RRT-Connect, to plan statically stable, collision free motions for humanoid robots. To assess the performance of both planning frameworks, we recorded the success rate for each task, the total time required to compute the motion plan, as well as the path length of the motion plans (the total sum of angular offsets in the trajectory). The standard deviation for the latter three metrics was also calculated. The results are provided in Table 1. 6.2. Planning with walking motion This experiment aims to demonstrate the ability of our planning framework to (a) compute motion plans for tasks in which 2 Video of task execution: https://youtu.be/CIy0PKHr7tQ.

Fig. 6. An illustration of the planning domain for the task in Section 6.2 which consists of all joints labeled in red . (For interpretation of the references to color in this figure legend, the reader is referred to the web version of this article.)

the goal is not within arm’s reach of the robot’s starting position, and (b) navigate through obstacles in a cluttered environment in order to reach the goal. In contrast to the previous section, the planning domain for this task fully includes the left leg, as well as the mimic joint. The left arm is also active, but remains static since the goal pose is defined for the end-effector in the right arm. Fig. 6 illustrates the planning domain for this task. Listed below is a description of the three tasks performed: 1. Shelf task 1: The environment setup is illustrated in Figs. 7a and 7b. Two obstacles are placed in front of the robot such that they create a passageway that is slightly narrower than the width of the robot. The goal pose is defined for the right arm, and is located on the lower level of a shelf (close to ground height). The shelf is placed such that its back is facing the robot in its starting state. In order to complete the task, the robot must navigate through the passageway by tilting its body, circle around the shelf to reach its front, and then finally bend down to reach the goal. 2. Shelf task 2: The environment setup is illustrated in Figs. 7c and 7d. The robot is partially facing an obstacle located at its front, and there is another obstacle to its right. The goal is located in a shelf just behind the obstacle in front such that the robot must maneuver around the obstacle in front without colliding with the obstacle on the right. 3. Table task: The environment setup is illustrated in Figs. 7e and 7f. The robot is facing a U-shaped obstacle, and must maneuver around it to reach the goal, which is located on a table just behind the obstacle. The motion plan generated by our planning framework can be visualized in Fig. 9. The details of the task execution are provided

R. Asif, A. Athar, F. Mehmood et al. / Robotics and Autonomous Systems 116 (2019) 51–63

59

Fig. 7. Environment setup shown from the side and top for the tasks in Section 6.2. The first, second and third rows illustrate the setup for shelf task 1, shelf task 2 and table task, respectively.

in Table 2. For this experiment, offering a direct comparison to other frameworks is not possible, since the latter do not have the same inputs and outputs as ours. For example, optimizationbased approaches [9–11] and the framework proposed in [5] require some initial trajectory. Aside from these, adapting currently available sampling-based planners to incorporate footstep planning is non-trivial because footstep planning requires an additional discrete state variable, namely the foot which is to be moved. This is something that vanilla sampling-based planners are currently not able to handle. 7. Discussion As discussed earlier, the performance of a motion planner can be assessed using the metrics of planning time, optimality and consistency. Regarding the latter, search-based algorithms guarantee the same solution for a given planning problem and

are, therefore, highly consistent. In terms of optimality, SMHA* guarantees bounded sub-optimality as long as the anchor heuristic is consistent and admissible. This condition is satisfied in our framework since we use the BFS 3D heuristic as the anchor. This improvement in terms of optimality is evident from the fact that our framework consistently achieved shorter path lengths than the comparison benchmark in all tasks in 6.1. 7.1. Planning with constant foot position In terms of planning time, the performance is dependent on the nature of the task. For the hole task in Section 6.1, Extended RRT-Connect performed poorly. This is because successful planning for this task requires the algorithm to sample points close to the hole. Given the random nature of the sampling process, this is unlikely, and hence the algorithm has a high failure rate and requires larger planning time. In general, sampling-based

60

R. Asif, A. Athar, F. Mehmood et al. / Robotics and Autonomous Systems 116 (2019) 51–63

7.2. Planning with walking motion

Fig. 8. Visualization of the contribution of each heuristic towards the final trajectory of shelf task 1. The images present a top-down view of an environment similar to Fig. 7b. The gray boxes depict obstacles, and the blue dots correspond to a particular heuristic discovering a graph state that eventually becomes part of the final trajectory. The dots are located at the mid-feet position of the robot according to that state, and the brightness of the dots reflects the relative number of such state discoveries (it is possible that multiple such states are discovered that do not involve foot-steps, so the robot’s mid-feet position remains constant). (i) BFS 3D heuristic, (ii) BFS 2D heuristic without obstacles inflated, (iii) BFS 2D heuristic with obstacles inflated, (iv) the combined results from both left and right foot island heuristics . (For interpretation of the references to color in this figure legend, the reader is referred to the web version of this article.)

algorithms perform poorly in tasks involving narrow passageways because of this reason. Similarly for the shelf task, Extended RRTConnect required considerable time in finding a path to reach the object in the top shelf. Because of obstacles in close proximity to the grasp object, and the large volume swept by an open endeffector, the planner had to adopt a specific approach path to accomplish this task. By contrast, Extended RRT-Connect out-performed our approach on the table task despite it being relatively simple. This is because the performance of heuristic search algorithms rapidly deteriorates when a local minimum is encountered. In this case, the minimum occurred when the robot leaned backward and reached the edge of stability. For the slit task, Extended RRTConnect’s superior performance came as a surprise given the narrow opening which has to be navigated through. Upon closer inspection, it was revealed that the slit was quite densely covered by the set of random states used by the sampling algorithm, which increased the likelihood of sampling a point close to the slit and thus finding a path through it. For all metrics, however, it can be seen that our approach yields lower variations. This is because graph search provides a higher degree of determinism, meaning that solutions to similar goal poses yield similar motion plans and planning times. For the sampling-based planner, the inherent randomization means that the resulting motion plans can vary greatly even for similar goal poses. Further discussion for a set of experiments similar to this can be found in our previous work [26].

With regard to the second set of experiments in 6.2, we demonstrate the ability of our planner to compute motion plans for challenging tasks by coalescing footsteps, obstacle avoidance and complex whole-body movements. This was made possible by utilizing multiple heuristics, each of which is designed to overcome a particular class of problems. A visualization of the contribution of each heuristic during the planning phase of shelf task 1 is given in Fig. 8. The BFS 2D heuristic helps to generate footstep motions that guide the robot through the narrow passageway. Thereafter, the second variant of this heuristic in which obstacles are inflated comes into play, and guides the robot around the shelf towards the front without colliding with the shelf. Finally, the BFS 3D heuristic helps to guide the end-effector to the goal position. The island heuristics appear to play an active role throughout the planning phase. Whenever one heuristic encounters a local minimum, another is able to continue searching, and because the states expanded by each heuristic are shared with the others, the former is eventually able to escape the local minimum. For example, while walking through the passageway, the obstacle inflated BFS 2D heuristic encounters a local minimum because the obstacle inflation significantly narrows the passageway, but the uninflated BFS 2D heuristic is still able to function correctly, and once the passageway has been cleared, the obstacle inflated heuristic is able to jump out of the minimum. After that, the uninflated BFS 2D heuristic gets stuck in a local minimum since it tries to make the robot turn too tightly around the shelf, causing a collision. Here, the obstacle inflated heuristic is able to function correctly and guides the robot to walk around a wider arc. A key contribution of our work is the utilization of island heuristics, which provide an intuitive and generalized way to simplify the planning process by introducing a lower dimensional sub-goal from where the final goal can be easily reached. The efficacy of this heuristic is evident from the density of its contribution to the final trajectory as shown in Fig. 8, and by the fact that if we run the planner with the island heuristic disabled, it altogether fails to generate a motion plan. Finally, commenting on the computation time, it is evident from the moderate number of state expansions involved that the algorithm converges fairly quickly, and so the large computation time must arise from other sources. Indeed, a significant portion of the total planning time (>59% for all three tasks) is spent on collision checking, since this is carried out using mesh models of the robot and environment. As an alternative, the mesh models can be approximated as a set of spheres and the resulting occupancy grid can be stored as a distance field. Collision checking can then be efficiently done using the radii of the spheres and the distance field. Aside from collision checking, significant time is also spent on IK computations (>14% for all three tasks), primarily since the application of a footstep primitive involves multiple such operations on a kinematic tree (see Appendix B for more details). The pre-processing time, which includes computation of the island foot poses and initialization of the occupancy grids for the heuristics, is fairly negligible. 8. Conclusion In this paper, we presented a whole body motion and footstep planning framework for humanoid robots that employs Shared Multi-heuristic A* (SMHA*) with a set of heuristics, each of which tackles commonly encountered problems faced during planning. In addition to commonly used heuristics employed by other frameworks, we employ a novel island heuristic that defines a lower dimensional sub-goal that greatly enhances the final goal’s

R. Asif, A. Athar, F. Mehmood et al. / Robotics and Autonomous Systems 116 (2019) 51–63

61

Fig. 9. Images of the execution of the motion plan for the experiment with walking motion. The photo sequence is ordered from left to right, and then top to bottom. Rows 1–2 show shelf task 1, rows 3–4 show shelf task 2, and rows 5–6 show the table task, as described in Section 6.2.

reachability. The graph is constructed dynamically, and state transitions are modeled by the application of one of multiple types of motion primitives that enable the planner to effectively coalesce footsteps with finer joint movements and intricate motions such as standing and sitting. We subsequently demonstrated the effectiveness of our framework by performing two separate experiments consisting of multiple, challenging tasks of a varied nature. Finally, we discussed and justified the results in light of the framework’s design. Possible future improvements include incorporation of dynamic waling motions [37] as opposed to statically stable footsteps, better handling of obstacles with overhangs, and implementation of efficient collision checking to reduce the total planning time. Appendix A. Database of feasible states In order to compute solutions to whole-body IK problems, we employ a numerical IK solver which accepts the target pose for the end-effector along with an initial starting joint configuration

(sometimes called a seed state) as input, and performs iterative optimization by computing the Jacobian matrix. One of the drawbacks of using this method is that the efficacy of the IK solver is heavily dependent on the choice of seed state. If the seed state is already close to the solution, then the IK solver will easily arrive at the solution. On the other hand, if the seed state is far-off, then it is possible that the IK solver will either encounter a singularity, or simply not converge within the maximum allowed number of iterations. For the case of agents with simple kinematic structures such as robotic arms or wheeled manipulation platforms, a simple technique to remedy the seed state problem is to re-attempt the IK problem from a different, randomly sampled seed state until a solution is found. For humanoid robots however, a randomly sampled joint configuration will most likely be statically unstable. To mitigate this issue, we build a database of statically stable, self-collision free joint configurations for the robot before-hand. Such a method has previously been used to reliably obtain 6D IK solutions for a 5-DoF robotic arm [38].

62

R. Asif, A. Athar, F. Mehmood et al. / Robotics and Autonomous Systems 116 (2019) 51–63

Our database actually consists of N different datasets, each corresponding to a particular foot pose. These N foot poses are chosen such that they offer a good coverage of the possible range of foot poses which the humanoid can attain. For each foot pose Fi defined by the position and orientation of one foot relative to the other (x, y, γ ), we create a dataset of valid joint configurations (i) Sdataset by iterating over the set of all possible joint configurations

S(i) , sampled at a reasonable resolution. A given state s ∈ S(i) (i) qualifies for membership in Sdataset if and only if:

• State s is not in self-collision. • All values in s respect the robot’s joint limits. • The foot pose of s is equivalent to Fi within some accuracy margin. When iterating over leg joint offsets, we attempt to force the foot pose to be Fi by adjusting the joint offsets of the other leg using IK. Thus, this IK problem must have a solution for s to be valid. (i)

The database Sdatabase = {Sdataset | i = 1, ., N } consisting of all datasets is then stored to disk. Along with the joint configuration of every s ∈ Sdatabase , the end-effector pose in cartesian coordinates is also stored along-side. During planning, when we wish to obtain promising seed states for an IK problem where the target end-effector pose is base Ttarget and the robot’s foot pose is Fj , we first select a dataset (j) Sdataset such that the L2 -distance between Fi and Fj is the smallest for all i ∈ [1, N ]. In the next step, we find a set of k feasible (j) seed states by searching all s ∈ Sdataset and selecting the k states for which the L2 distance between their end-effector poses and base Ttarget is smallest. (i) In our implementation, N = 11, and each dataset Sdataset contains approximately 300,000 valid states. To perform efficient search, we employ the Fast Library for Nearest Neighbor Approximation (FLANN) [39] which organizes the states into a KD-Tree. Even on a mediocre laptop CPU, nearest neighbor retrieval can be performed in sub-millisecond time. The source code for generating this database and applying it to solve IK problems for the Nao robot is available online.3 Appendix B. Walking pattern generator In order to perform footsteps, we developed a simple, static biped walking pattern generator inspired by the popular 3D linear inverted pendulum model [40]. The reason we did not use a Zero-Moment Point [41] based dynamic pattern generator is that it would considerably increase the dimensionality of the graph since additional state variables would be needed to model the dynamics of motion. Furthermore, while dynamic motions offer better mobility, static motions also have certain benefits, e.g., the motion can be paused at any point and planning can be restarted from scratch in case the environment changes. The following nomenclature will be used from here onward: the foot which has to be moved will be referred to as the swing foot, whereas the other will be called the support foot. The Center of Mass of the robot is abbreviated as ‘CoM’. In our framework, a footstep is defined in terms of offsets to the position and orientation of the swing foot: (∆x, ∆y, ∆γ ). We assume that the movement of the robot is sufficiently slow such that dynamic effects do not come into play i.e. ensuring that the CoM lies within the robot’s support polygon is a necessary and sufficient condition for stability. The footstep is performed by executing the following three steps in order: 3 https://github.com/Ali2500/nao_whole_body_ik.

1. Shifting the CoM to the fixed foot. This implies that the ground projection of the CoM, i.e. its (x, y) coordinates, lie inside the hull of the support foot. 2. Moving the swing foot as required by (∆x, ∆y, ∆γ ). This can be done since the support foot can safely support the entire robot’s weight. The actual trajectory of the swing foot is modeled after a teardrop curve since it closely resembles human-like footstep motion. The required swing foot position at various points along the curve is sampled at a suitable resolution. Then, in order to find a joint configuration for each position, IK is applied on the swing foot with an additional constraint that the CoM must remain above the support foot (the CoM position is recomputed after every movement along the points of the curve). 3. Shifting the CoM such that its (x, y) coordinates lie roughly at the mid-feet position. This is done to restore the natural standing pose of the robot. To shift the CoM, we model it as a virtual end-effector attached to the robot’s torso via a temporary link, and then apply IK to compute the joint movement required to move the CoM to its desired location. This is a coarse approximation since the CoM actually changes position as the motion is being undertaken, so to compensate, we shrink the support foot’s hull by 30% as this factor was empirically found to be suitable for maintaining stability during task execution. Moreover, it functions well since the CoM need not necessarily be exactly at the center of the shrinked hull, but rather anywhere inside it. Similarly, in step 3, when the CoM is shifted back to the mid-feet position, there is a generous error margin since we are not aiming for an exact solution. Appendix C. Supplementary data Supplementary Video of the experiments discussed in Section 6.2 can be found online at http://dx.doi.org/10.1016/j.robot. 2019.03.007. References [1] E. Yoshida, I. Belousov, C. Esteves, J.-P. Laumond, Humanoid motion planning for dynamic tasks, in: Humanoid Robots, 2005 5th IEEE-RAS International Conference on, IEEE, 2005, pp. 1–6. [2] O. Kanoun, F. Lamiraux, P.-B. Wieber, F. Kanehiro, E. Yoshida, J.-P. Laumond, Prioritizing linear equality and inequality systems: application to local motion planning for redundant robots, in: Robotics and Automation, 2009. ICRA ’09. IEEE International Conference on, 2009, pp. 2939–2944, http: //dx.doi.org/10.1109/ROBOT.2009.5152293. [3] O. Kanoun, J.-P. Laumond, E. Yoshida, Planning foot placements for a humanoid robot: a problem of inverse kinematics, Int. J. Robot. Res. 30 (4) (2011) 476–485. [4] J. Pettré, J.-P. Laumond, T. Siméon, A 2-stages locomotion planner for digital actors, in: Proceedings of the 2003 ACM SIGGRAPH/Eurographics Symposium on Computer Animation, Eurographics Association, 2003, pp. 258–264. [5] M. Cognetti, P. Mohammadi, G. Oriolo, Whole-body motion planning for humanoids based on com movement primitives, in: 2015 IEEE-RAS 15th International Conference on Humanoid Robots (Humanoids), 2015, pp. 1090–1095, http://dx.doi.org/10.1109/HUMANOIDS.2015.7363504. [6] M. Diehl, H.G. Bock, H. Diedam, P.-B. Wieber, Fast direct multiple shooting algorithms for optimal robot control, in: Fast motions in biomechanics and robotics, Springer, 2006, pp. 65–93. [7] K. Mombaur, Using optimization to create self-stable human-like running, Robotica 27 (3) (2009) 321–330. [8] M. Posa, R. Tedrake, Direct trajectory optimization of rigid body dynamical systems through contact, in: Algorithmic Foundations of Robotics X, Springer, 2013, pp. 527–542. [9] N. Ratliff, M. Zucker, J.A. Bagnell, S. Srinivasa, CHOMP: gradient optimization techniques for efficient motion planning, in: Robotics and Automation, 2009. ICRA’09. IEEE International Conference on, IEEE, 2009, pp. 489–494.

R. Asif, A. Athar, F. Mehmood et al. / Robotics and Autonomous Systems 116 (2019) 51–63 [10] M. Kalakrishnan, S. Chitta, E. Theodorou, P. Pastor, S. Schaal, Stomp: stochastic trajectory optimization for motion planning, in: 2011 IEEE International Conference on Robotics and Automation, 2011, pp. 4569–4574, http://dx.doi.org/10.1109/ICRA.2011.5980280. [11] J. Schulman, J. Ho, A.X. Lee, I. Awwal, H. Bradlow, P. Abbeel, Finding locally optimal, collision-free trajectories with sequential convex optimization., in: Robotics: Science and Systems, Vol. 9, 2013, pp. 1–10. [12] J.J. Kuffner Jr, S. Kagami, K. Nishiwaki, M. Inaba, H. Inoue, Dynamicallystable motion planning for humanoid robots, Auton. Robots 12 (1) (2002) 105–118. [13] S. Dalibard, A. Nakhaei, F. Lamiraux, J.-P. Laumond, Whole-body task planning for a humanoid robot: a way to integrate collision avoidance, in: Humanoid Robots, 2009. Humanoids 2009. 9th IEEE-RAS International Conference on, IEEE, 2009, pp. 355–360. [14] N. Perrin, O. Stasse, L. Baudouin, F. Lamiraux, E. Yoshida, Fast humanoid robot collision-free footstep planning using swept volume approximations, IEEE Trans. Robot. 28 (2) (2012) 427–439. [15] N. Perrin, O. Stasse, F. Lamiraux, E. Yoshida, Humanoid motion generation and swept volumes: theoretical bounds for safe steps, Adv. Robot. 27 (14) (2013) 1045–1058. [16] K.H. Teja, F. James, S.V. Shah, Optimal whole body motion planning of humanoid with articulated spine for object manipulation in double support phase, in: Proceedings of the 2015 Conference on Advances In Robotics, ACM, 2015, p. 30. [17] J. Baltes, J. Bagot, S. Sadeghnejad, J. Anderson, C.-H. Hsu, Full-body motion planning for humanoid robots using rapidly exploring random trees, KI-Künstliche Intell. 30 (3–4) (2016) 245–255. [18] J.E. King, M. Cognetti, S.S. Srinivasa, Rearrangement planning using objectcentric and robot-centric action spaces, in: Robotics and Automation (ICRA), 2016 IEEE International Conference on, IEEE, 2016, pp. 3940–3947. [19] P. Ferrari, M. Cognetti, G. Oriolo, Humanoid whole-body planning for loco-manipulation tasks, in: Robotics and Automation (ICRA), 2017 IEEE International Conference on, IEEE, 2017, pp. 4741–4746. [20] F. Burget, A. Hornung, M. Bennewitz, Whole-body motion planning for manipulation of articulated objects, in: Robotics and Automation (ICRA), 2013 IEEE International Conference on, IEEE, 2013, pp. 1656–1662. [21] J. Chestnutt, M. Lau, G. Cheung, J. Kuffner, J. Hodgins, T. Kanade, Footstep planning for the honda asimo humanoid, in: Proceedings of the 2005 IEEE International Conference on Robotics and Automation, 2005, pp. 629–634, http://dx.doi.org/10.1109/ROBOT.2005.1570188. [22] J. Garimort, A. Hornung, M. Bennewitz, Humanoid navigation with dynamic footstep plans, in: Robotics and Automation (ICRA), 2011 IEEE International Conference on, IEEE, 2011, pp. 3982–3987. [23] B.J. Cohen, S. Chitta, M. Likhachev, Search-based planning for manipulation with motion primitives, in: Robotics and Automation (ICRA), 2010 IEEE International Conference on, IEEE, 2010, pp. 2902–2908. [24] B.J. Cohen, G. Subramanian, S. Chitta, M. Likhachev, Planning for manipulation with adaptive motion primitives, in: Robotics and Automation (ICRA), 2011 IEEE International Conference on, IEEE, 2011, pp. 5478–5485. [25] S. Aine, S. Swaminathan, V. Narayanan, V. Hwang, M. Likhachev, Multi-heuristic A*, Int. J. Robot. Res. 35 (1–3) (2016) 224–243. [26] A. Athar, A.M. Zafar, R. Asif, A.A. Khan, F. Islam, Yasar, O. Hasan, Wholebody motion planning for humanoid robots with heuristic search, in: 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2016, pp. 4720–4727, http://dx.doi.org/10.1109/IROS.2016.7759694. [27] F. Islam, O. Salzman, M. Likhachev, Online, interactive user guidance for high-dimensional, constrained motion planning, CoRR abs/1710.03873. arXiv:1710.03873. URL http://arxiv.org/abs/1710.03873. [28] A. Dornbush, K. Vijayakumar, S. Bardapurkar, F. Islam, M. Likhachev, A single-planner approach to multi-modal humanoid mobility, CoRR abs/1801.10225. arXiv:1801.10225. URL http://arxiv.org/abs/1801.10225. [29] P. Chakrabarti, S. Ghose, S. DeSarkar, Heuristic search through islands, Artificial Intelligence 29 (3) (1986) 339–347, http://dx.doi.org/10.1016/ 0004-3702(86)90074-3, URL http://www.sciencedirect.com/science/article/ pii/0004370286900743. [30] J.F. Dillenburg, Techniques for Improving the Efficiency of Heuristic Search (Ph.D. thesis), University of Illinois at Chicago, 1993. [31] P.E. Hart, N.J. Nilsson, B. Raphael, A formal basis for the heuristic determination of minimum cost paths, IEEE Trans. Syst. Sci. Cybern. 4 (2) (1968) 100–107. [32] M. Likhachev, SBPL graph search library. [33] S. Chitta, I. Sucan, S. Cousins, Moveit! [ROS Topics], IEEE Robot. Autom. Mag. 19 (1) (2012) 18–19, http://dx.doi.org/10.1109/MRA.2011.2181749. [34] J. Pan, S. Chitta, D. Manocha, FCL: a general purpose library for collision and proximity queries, in: Robotics and Automation (ICRA), 2012 IEEE International Conference on, IEEE, 2012, pp. 3859–3866. [35] R. Smits, H. Bruyninckx, E. Aertbeliën, KDL: kinematics and dynamics library, Avaliable: http://www.orocos.org/kdl. [36] V. Narayanan, S. Aine, M. Likhachev, Improved multi-heuristic a* for searching with uncalibrated heuristics, in: Eighth Annual Symposium on Combinatorial Search, 2015.

63

[37] R.D. Gregg, A.K. Tilton, S. Candido, T. Bretl, M.W. Spong, Control and planning of 3-d dynamic walking with asymptotically stable gait primitives, IEEE Trans. Robot. 28 (6) (2012) 1415–1423. [38] A. Henning, Approximate Inverse Kinematics Using a Database. [39] M. Muja, D.G. Lowe, Fast approximate nearest neighbors with automatic algorithm configuration., VISAPP (1) 2 (2009) 331–340. [40] S. Kajita, F. Kanehiro, K. Kaneko, K. Yokoi, H. Hirukawa, The 3d linear inverted pendulum mode: a simple modeling for a biped walking pattern generation, in: Proceedings 2001 IEEE/RSJ International Conference on Intelligent Robots and Systems. Expanding the Societal Role of Robotics in the the Next Millennium (Cat. No. 01CH37180), Vol. 1, 2001, pp. 239–246, http://dx.doi.org/10.1109/IROS.2001.973365, vol.1. [41] M. Vukobratović, B. Borovac, Zero-moment pointthirty five years of its life, Int. J. Humanoid Robot. 1 (01) (2004) 157–173.

Rizwan Asif is an Autonomous Systems EIT Digital master’s student at KTH Royal Institute of Technology, Sweden and Aalto University, Finland. He completed his bachelor’s degree in Electrical Engineering from National University of Science and Technology, Islamabad, Pakistan. There he was associated with the Robotics and Intelligent Systems Engineering Laboratory (RISE). His research areas include artificial intelligence, robot motion planning, and humanoid robots. Ali Athar is a master’s student at the Technical University of Munich in Germany. He completed his bachelor’s degree in Electrical Engineering from the National University of Sciences and Technology in Islamabad, Pakistan and graduated with the second-highest score in his class. Currently, he also works as a software developer in 3D Robotics and Perception at NavVis GmbH. Ali’s areas of interest include robotic motion planning, computer vision and machine learning. Faisal Mehmood has received his Bachelor of Engineering in Electrical Engineering from COMSATS Institute of Information Technology, Pakistan in 2016. He received Master of Science in Robotics and Intelligent Machine Engineering from dept. of Robotics and Intelligent Machine Engineering, School of Mechanical and Manufacturing Engineering, National University of Sciences and Technology, Pakistan in 2018. Currently, he is serving as a Research Engineer and Lab Engineer at Dept. of Robotics and Intelligent Machine Engineering, School of Mechanical and Manufacturing Engineering, National University of Sciences and Technology, Islamabad, Pakistan. Fahad Islam is a PhD candidate at the Robotics Institute in Carnegie Melon University. He completed his bachelor’s degree from the National University of Sciences and Technology is Islamabad, Pakistan. After that, he was awarded a Fulbright Scholarship to attend the MS Robotics program at Carnegie Melon University. His research areas include Artificial Intelligence and Robotics, more specifically high dimensional motion planning for real-time robotics applications. Yasar Ayaz received his B.Eng. degree in Mechatronics Engineering and M.Eng. degree in Electrical Engineering from the National University of Sciences and Technology (NUST), Pakistan, in 2003 & 2005, respectively. He received his PhD. in Robotics and Machine Intelligence from Tohoku University, Japan in 2009. He is currently working as the HoD Robotics and A.I at the School of Mechanical and Manufacturing Engineering, NUST, Pakistan. Alongside, He is the Chairman of Pakistan’s National Center of Artificial Intelligence. And since 2015 he is the Specially-Appointed Associate Professor at Tohoku University, Japan. His research includes motion-planning, humanoid navigation and control and mobile robots.