NSPG: An Efficient Posture Generator Based on Null-Space Alteration and Kinetostatics Constraints

Abstract

Most of the locomotion and contact planners for multi-limbed robots rely on a reduction of the search space to improve the performance of their algorithm. Posture generation plays a fundamental role in these types of planners providing a collision-free, statically stable whole-body posture, projected onto the planned contacts. However, posture generation becomes particularly tedious for complex robots moving in cluttered environments, in which feasibility can be hard to accomplish. In this work, we take advantage of the kinematic structure of a multi-limbed robot to present a posture generator based on hierarchical inverse kinematics and contact force optimization, called the null-space posture generator (NSPG), able to efficiently satisfy the aforementioned requisites in short times. A new configuration of the robot is produced through conservatively altering a given nominal posture exploiting the null-space of the contact manifold, satisfying geometrical and kinetostatics constraints. This is achieved through an adaptive random velocity vector generator that lets the robot explore its workspace. To prove the validity and generality of the proposed method, simulations in multiple scenarios are reported employing different robots: a wheeled-legged quadruped and a biped. Specifically, it is shown that the NSPG is particularly suited in complex cluttered scenarios, in which linear collision avoidance and stability constraints may be inefficient due to the high computational cost. In particular, we show an improvement of performances being our method able to generate twice feasible configurations in the same period. A comparison with previous methods has been carried out collecting the obtained results which highlight the benefits of the NSPG. Finally, experiments with the CENTAURO platform, developed at Istituto Italiano di Tecnologia, are carried out showing the applicability of the proposed method to a real corridor scenario.

1 Introduction

Achieving autonomous whole-body motion behaviors for humanoid and legged robots still represents a challenging research topic. The capability to take autonomous decisions while walking or manipulating objects is fundamental for complex platforms working in real and uncertain environments, without the need for human supervision and intervention Polverini et al. (2020). Effective motion planning on highly redundant robots with a large number of degrees of freedom (DoFs) requires satisfying multiple objectives and constraints concerning loco-manipulation and stability while avoiding internal (self) and external collisions with the environment. Indeed, the computational cost of a motion planning algorithm dramatically increases depending on the dimension of the state space (i.e., DoF number). For motion planners who directly search on joint space configurations, this could often lead to the impossibility of finding a solution within a reasonable time (a.k.a. curse of dimensionality). To overcome this issue, previous works adopt simplifying assumptions to reduce the state space dimension or use a discrete control space (i.e., actions) Cognetti et al. (2015); Ferrari et al. (2018); Hauser et al. (2008). However, these methods suffer from the trade-off between a small action set, which can reduce the branching factor of the search tree inhibiting specific motions, and a large action set, which increases the branching factor of the search tree that becomes harder to explore. Alternatively, planners based on continuous optimization were used in Deits and Tedrake (2014); Kuindersma et al. (2016); Ratliff et al. (2009), but they do not guarantee completeness or global optimality and it is non-trivial to generate optimal collision-free trajectories within time frames acceptable for online planning, especially in complex environments.

A further possibility is to use footstep/multi-contact plannersBouyarmane and Kheddar (2012); Hauser et al. (2005); Kuffner et al. (2001); Tonneau et al. (2018), which have been widely applied in biped robots. In these approaches, the state space is reduced to consider only the position and orientation of each contact as a working variable. The price to pay is the necessity to move back to the configuration space through a map that associates a whole-body configuration of the robot with a specific set of contacts (i.e., stance) coming from the planner. Indeed, collision with the environment, self-collision, and equilibrium are constraints to be considered when generating a posture projected onto the planned contacts, with the risk of invalidating the sampled state if a feasible configuration cannot be found. Furthermore, the configurations must be generated in such a way that they guarantee a feasible transition motion between them, which is one of the most strict requirements to satisfy. This is done from both stability and collision safeness points of view and will be better explored in the next sections. Hence, footstep and multi-contact planners rely heavily on posture generators, which have to not only satisfy the aforementioned constraints but also be able to generate new postures efficiently.

Addressing these concerns, in this work, we propose a novel posture generator algorithm based on the hierarchical inverse kinematics (HIK), called the null-space posture generator (NSPG), able to generate collision-free and statically balanced configurations for arbitrarily complex floating-base robots. In particular, the NSPG exploits the null-space of the robot, which is used to locally correct its posture around a nominal configuration, generated starting from the history of previously computed feasible postures. In our method, the previous configuration becomes also the seed configuration of the HIK solver, guaranteeing minimal differences between adjacent postures. If a chain of the robot is in contact with the environment or in self-collision, instead of generating a new whole posture, only the involved kinematic chain is moved to avoid the collision, in the neighborhood of the nominal configuration. Hence, we take advantage of the kinematic structure of multi-limbed robots to restore feasibility. Differently, when the generated posture is statically unstable, only the root link is moved to restore feasibility.

The method, compared with previous works, contributes with a smart selection of the seed and nominal configurations for the HIK solver, seeking for a new feasible one inside a small workspace around the nominal configuration whose volume is defined by the parameters of the algorithm. Specifically, it exploits the robot workspace in random directions, moving in the neighborhood of the nominal configuration. This allows the posture generator to look for a feasible configuration instead of discarding the state as soon as the first computed configuration is unfeasible, improving the performance of the planner and the algorithm itself. Last, the number of parameters to be tuned is kept minimum and is not dependent neither on the environment nor on the robot (discussed in detail in Section 5). A similar approach has been used in Yang et al. (2016) in which feasible postures were generated using the IK randomly sampling seed configurations from the balanced manifold. However, stability is checked using the projection of the CoM onto the support polygon drawn by feet, thus not considering non-co-planar contacts which were included in the follow-up work Yang et al. (2017) and Ferrolho et al. (2018). The proposed method is first validated in simulations on the hyper-redundant hybrid wheeled-legged quadrupedal robot CENTAURO Kashiri et al. (2019); Figure 1 and on the biped robot COMAN+ Ruscelli et al. (2020) to prove the generality of the algorithm independently from the number of considered end-effectors or complexity of the robotic platform. Both CENTAURO and COMAN+ are developed at Istituto Italiano di Tecnologia. An experimental assessment of the proposed method is also carried out employing CENTAURO’s perception system (Lidar sensor) for perceiving the environment in front of the robot.

FIGURE 1

This paper introduces the literature on previous related approaches and novel contributions to posture generation applied to contacts and whole-body motion planning in Section 2. The methodology is explained in Sections 3 and 4 that will be used later for the algorithm description in Section 5. To conclude, Sections 6 and 7 describe the obtained simulation and experimental results and how the performance of the introduced method compares to that of the previous work.

2 Related Work

The generation of feasible whole-body configurations for a legged robot coupled with footsteps or multi-contact planners has been widely investigated in the past years. The previous work is based on pre-computed paired forward–inverse dynamic reachability maps (DRM/iDRM) to sample in the reachable workspace those configurations that could accomplish a loco-manipulation task while guaranteeing stability and collision safeness on flat Yang et al. (2017) and inclined Ferrolho et al. (2018) terrains. This method is characterized by big computational and memory costs which are reduced, solving for the upper and lower bodies separately. Additionally, it requires the computation and discretization of the reachable workspace which becomes computationally heavier when the number of contacts increases.

In Hauser et al. (2005), an iterative constraint enforcement (ICE) algorithm was used to generate statically stable and collision-free configurations using the Newton–Raphson method. The generated postures are subject to Cartesian constraints for the contacts and CoM position to guarantee stability, starting from randomly sampled initial configurations. However, random seed configurations do not take into account the problem of minimal displacement between adjacent postures, and this could lead to unfeasibilities during the transition motion.

Other approaches explore fixed-size Gutmann et al. (2005); Deits and Tedrake (2014) or variable-size Buchanan et al. (2019) bounding boxes to find the best collision-free walking posture. This whole-body posture is then projected onto the contacts found during the planning phase. However, these methods do not take advantage of the capability of reshaping the whole body of the robot to facilitate and eventually permit transiting in scenarios where the dimensions of the free passage are closely the physical dimensions of the robot body.

In Bouyarmane and Kheddar (2012), a non-linear optimization has been used to compute IK with static stability, collision avoidance, torque limits, and joint limits as constraints. In this work, no further modifications are carried out when the solver is not able to find a solution, leading to a possible avoidable discard of the sampled contact state.

A different approach was used in Tonneau et al. (2018). The contact planner problem is addressed first by finding a guide path for the floating base in the configuration space while satisfying a reachable condition to guarantee collision safeness and workspace reachability of the end-effectors. Then, a sequence of discrete configurations is computed using an iterative algorithm that satisfies a specific contact transition, stability, and collision safeness starting from the root guide path. Ultimately, the contact sequence is retrieved from the configuration sequence. However, this method relies on a pipeline that may suffer from a necessary fine-tuning of its parameters, especially for the effectiveness of the reachable condition which strictly depends on the kinematic characteristics of the robot.

Recently, in Shigematsu et al. (2019), a posture generator has been developed to plan whole-body trajectories for a humanoid robot moving heavy suitcases. The approach is based on a non-linear program where several key postures are optimized all together with the centroidal statics, joint limits, and self-collision constraints. Despite the impressive results obtained on the real platform, the method does not account for environmental collisions, and it still needs several minutes to compute a sequence of configurations.

3 Methodology

The NSPG aims to generate collision-free whole-body configurations realizing both kinematic and statics constraints that will be detailed in Sections 3.2 and 3.3. These constraints arise from the contacts that the robot is required to establish with the environment to execute an assigned task and are generally the output of a contact planner (Section 3.1). To this end, we introduce in this section the basic notions that will be used in the following.

3.1 Stance Generation

For each state , given the stance , the posture generator aims to find a feasible configuration compliant with . The path of stances is in turn found by a generic footstep or multi-contact planner. However, the description of the planner algorithm is out of the scope of this work.

The configuration space velocities associated with the configurations are denoted that contain the joint space velocities and the linear and angular base velocities:

In addition, with proper validity functions, we assume to sample stances with poses of the contacts in the workspace of the robots and not inside any obstacle.

3.2 Hierarchical IK

The Cartesian velocity of an end-effector frame w.r.t. a reference is related to through the relationwhere is the Jacobian¹ of the frame w.r.t. expressed in .

The inverse problem of Eq. 3, a.k.a. differential inverse kinematics, permits to compute the configuration velocities which realize a desired Cartesian velocity for a certain end-effector. The computation of is classically found solving a least-square problem in the formwith being a weight matrix. In order to track Cartesian poses as well, a closed loop IK (CLIK) scheme is often employed where the desired Cartesian velocity is set aswith being a feed-forward Cartesian velocity reference, being a reference Cartesian pose, being the actual Cartesian pose, and λ being a gain which ensures exponential convergence of the Cartesian error to zero. The configuration velocities computed using Eq. 4 can be integrated to obtain the new robot configuration, through the integration function :

The problem in Eq. 4 can be formulated as a quadratic programming (QP) problem with the main advantage of considering equality and inequality constraints as well Kanoun et al. (2011):

Furthermore, hard priorities between tasks can be enforced in the QP-based IK by means of a cascade of QPs Kanoun et al. (2009) or using particular hierarchical orthogonal decomposition of the aggregated task matrices Escande et al. (2014).

We organize these tasks and constraints in the following stack :where the “∑” symbol means that all the contact tasks are summed at the same priority level and the “/” symbol means that the postural task acts in the null-space of the contact tasks and is, hence, the “hierarchical” term in HIK. The “” symbol means that all the tasks are subject to the joint limits constraint. This formulation, known as math of tasks, follows the work done in Mingo Hoffman and Tsagarakis (2021).

3.3 Centroidal Statics

To grant quasi-static stability for a given robot configuration , compliant with a stance , a critical role is played by the interaction forces. Static stability is checked solving another QP based on the stances’ information of contact position , its associated normal , and CoM position computed from the configuration to be checked.

The resulting QP in the variables , with being all the contact wrenches w.r.t. the inertial frame², is formulated as

The first term in Eq. 14a ensures static stability, under quasi-static conditions, based on the centroidal statics (CS) of the robot, where the terms and are the vector of the gravity acceleration and momentum variation and the mass of the robot, respectively, and is the centroidal dynamics grasp matrix:with being the skew-symmetric matrix operator.

This reduced description assumes fixed contact placements with associated linearized friction models and unilaterality of the contact force Eq. 14c, center of pressure (CoP) inside the contact surface Eq. 14d, and bounded contact yaw torque Eq. 14eCaron et al. (2015)³, to obtain the interaction forces required to compensate for gravity, achieving static balancing and non-slippage of surface contacts. Matrices , , and are expressed as , , and withwhere and are the coefficient matrices of the constraint inequalities expressed in the local force frame and is the adjoint rotation matrix that transforms the wrench from the ith local frame to the inertial frame , computed from the contact normal . In particular, is the static friction coefficient associated with the ith contact, x and y are half the size of the surface contact⁴, and is the rotation matrix that moves from the ith contact frame to the inertial frame .

It is worth noticing that modeling a surface contact using forces and moments, together with the constraints Eqs. 14c–e, permits to save variables and constraints. Assuming four contact points per surface contact leads to a total of 12 pure contact force variables and 20 constraints in the form of Eq. 16a. On the contrary, assuming a single wrench leads to 6 variables to describe contact forces and torques and 17 constraints. A graphical representation of the centroidal statics’ components is given in Figure 2.

FIGURE 2

The residual of the first term in the cost function Eq. 14a is used to decide whether a configuration is stable or not when satisfying a specific stance, depending on a threshold value.

4 Generating Transition Configurations

In a contact planner application case, a feasible sequence of adjacent postures is required to be connectable in order to build a configuration path that moves the robot safely from a start configuration to a goal configuration . In particular, two configurations and are connectable if there exists a continuous path satisfying , , and the requirements of stability and collision avoidance. Furthermore, a local planner interpolator guarantees a feasible trajectory between two consecutive configurations. Having defined , the set of all configurations that satisfies , we can say that consecutive configurations are connectable if such thatwith being the set of continuous functions. Collision avoidance is sought generating similar adjacent poses, thus minimizing the transition motion that moves the robot from to . In order to better understand this last requirement, imagine a robot side-walking in a narrow space. In this scenario, feasible postures can be the one with the robot facing both leftward and rightward. However, if two adjacent and contain configurations that face opposite sides of the narrow passage, the transition motion between and will probably collide with the environment, see Figure 3. We solve this issue using the parent state’s configuration as a nominal configuration for the generation of , thus forcing to be in a small neighborhood of (Section 5). This assumption works in the hypothesis of small changes of the environment seen by the robot, which covers the most of the considered scenarios. Indeed, when moving in such an environment, the previous feasible configuration is a first good guess to generate the next feasible configuration. In this way, any transition motion is generated only if required.

FIGURE 3

Furthermore, in the assumption that near stances differ by exactly one active contact, the generated configuration must be statically stable w.r.t. the minimum contact number stance between and to guarantee the existence of a stable trajectory between the two stances Escande et al. (2013).

5 Null-Space Posture Generator

Our approach is based on a complete reshape of the robot configuration, obtained by adjusting the pose of kinematic chains in a collision, or moving the root link to recover the static stability, in the null-space of the Cartesian (contact) tasks. Specifically, each detected unfeasibility will generate random velocity components aiming to recover feasibility. This section follows a pipeline going through each component of the algorithm: first, the nominal configuration and the random velocity vector are generated. The NSPG exploits the neighborhood of the nominal configuration in the random direction defined by . Then, the procedure to adapt the velocity vector and the candidate configuration procedures are described. The strategy used is described in Algorithm 1, while a graphical representation is given in Figure 4.

FIGURE 4

ALGORITHM 1:

NSPG( ).

5.1 Nominal Configuration Generation

First, the candidate configuration is computed projecting the seed configuration onto the manifold defined by the stance . The projection is performed by the HIK solving the stack in Eq. 13 with the same used as a reference for the postural task (line 4)⁵. In the case the first candidate configuration is feasible, and no further adjustment is required, the NSPG will return . Oppositely, will be used as the nominal configuration in which neighborhood the NSPG will look for a new feasible configuration.

5.2 Adaptive Random Velocity Vector Generation

In the following, the procedure to generate the random velocity vector will be described, depending on unfeasibility.

5.2.1 Collisions

In this phase, we take advantage of the kinematic structure of a multi-limbed robot to avoid collisions while keeping low differences between adjacent configurations. Indeed, the motion of a kinematic chain is independent concerning the others, assuming that its end-effector is not constrained onto a contact pose. In this way, we can freely move only the chain(s) involved in unfeasibility, preventing avoidable motions that could lead to other collisions.

In detail, when is in (self-)collision, the colliding kinematic chains are detected (line 5) and collected into the set C. The kinematic chain is defined as the set of links and joints moving from the tip link (i.e., end-effector) to the base link. For each of the joints belonging to those chains, a random bounded velocity vector is generated as written in line 16. The random velocity vector comes from a uniform random distribution bounded between the joint velocity limits. returns a vector that extracts the aforementioned joints from the whole joints’ list.

It is worth mentioning that collisions/self-collisions can be avoided as well as integrating specific cost functions or constraints in the HIK Fang et al. (2015); Stasse et al. (2008). Despite appealing, this inclusion may have a non-negligible computational cost which is avoidable when HIK is used as a posture generator. The increase in computational cost grows together with the complexity of the surrounding environment which makes the active set computationally expensive. For kinematic chains fully constrained onto the set of contacts defined by , their reshape is obtained through a linear displacement of the root link as shown in line 14.

With this methodology, the NSPG moves only the joints involved in unfeasibility preventing avoidable motions of the rest of the body.

5.2.2 Stability

A second scenario occurs when is not statically stable (line 19). The static stability is checked solving the QP problem as written in line 14 comparing the residual of the first term of the cost function with a threshold value .

In this case, linear velocities of the root link are generated from a uniform random distribution bounded between two arbitrarily big numbers ( in our case). This limit value is decided to be arbitrarily big since the postural task is at the lowest level of the HIK solver. Indeed, to obtain a visible motion of the base link, when moving the floating base to recover stability with the specific parameter set chosen, it has to be moved fast enough.

It has been chosen to move the root link instead of the CoM directly since a motion of the latter could imply an undesired whole-body motion involving non-colliding kinematic chains or deviating the motion of the colliding ones unpredictably.

5.3 Candidate Configuration Update

The postural task reference is then updated according to the new velocity vector (line 25), and the HIK is solved generating a new robot configuration according to the new postural reference (line 26). This procedure is repeated every iterations, until a feasible pose is found, according to and which are chosen to be sufficiently small to guarantee the stable configurations, well-projected onto the contact manifold. The algorithm exploits the robot workspace in the direction defined by for iterations, after which the reference posture of the robot is reset to the starting one (line 10).

5.4 Velocity Vector Adaptation

While running, the algorithm will generate configurations with arbitrarily small differences which depends on its parameters and velocity vector .

Collision and stability checks strictly depend on the current candidate configuration of the robot that changes at each iteration of the algorithm. Thus, the velocity vector must be updated and adapted depending on the state of the robot throughout each iteration.

Specifically, at each iteration, the colliding chains are updated, and two new sub-sets are defined:with and containing the set of the new and old colliding chains, respectively, depending on the new candidate configuration. In correspondence with , random velocity components are added to , while velocity components are removed depending on :with returning a vector containing 1 in correspondence with the joints belonging to the new colliding chains and 0 elsewhere, while returns a vector containing 0 in correspondence with the old colliding chains’ joints and 1 elsewhere. is the old joint velocity vector for the actuated joints, and the operator defines a component-wise product between two vectors of the same size. Additionally, stability can be lost or recovered while generating new candidate configurations. In these cases, the velocity vector must be updated adding or removing velocity components corresponding to linear root link velocities as follows:

At each iteration, this method looks for a feasible configuration inside a maximum allowable workspace around the nominal configuration. The maximum volume of the robot workspace is defined by the parameters and , multiplied times the maximum velocity vector containing the absolute value of the velocity limits of each joint. The bigger these values, the bigger the maximum explorable workspace, allowing for bigger differences between adjacent configurations. While the maximum velocity vector strictly depends on the mechanical characteristics of the robot, the two parameters and were tuned after several trials with different parameter sets, picking the best obtained result. The NSPG algorithm moves only the joints involved in the unfeasibility of about a quantity that depends on the NSPG parameters, listed in Table 1. Increasing allows the robot to explore a larger range of motion around the nominal configuration. The parameter is the integration time involved in line 25: the smaller this parameter, the smaller the motion between two generated configurations during a single call of the NSPG. In addition, keeping constant, the integration time will also influence the maximum range of motion around the nominal configuration. Ultimately, the timeout sets a time threshold for the search of a feasible configuration.

6 Results

The proposed NSPG algorithm has been tested in two scenarios with increasing difficulty, applied on two different types of legged hyper-redundant robots: the hybrid wheeled-legged quadrupedal robot CENTAURO and the biped robot COMAN+. CENTAURO is a robot with 39 DoFs split between a quadrupedal lower body and a bimanual humanoid upper body, while COMAN+ is a biped humanoid robot with 28 DoFs.

The first considered scenario consists of multiple tiles, placed at different heights and orientations, where the robot has to step on or place its limbs, while the second one is a narrow corridor on a flat terrain. Additionally, an experiment of this last scenario has been carried out on CENTAURO.

Our NSPG implementation is based on the OpenSoT Hoffman et al. (2017) and CartesI/O Laurenzi et al. (2019) frameworks for the computation of the whole-body HIK and centroidal statics QP problems, depicted in Sections 3.2 and 3.3, respectively. In particular, QPs are efficiently solved using well-known QP solvers such as qpOASESFerreau et al. (2014) or OSQPStellato et al. (2020). Collisions are detected exploiting the Flexible Collision Library (FCL) Pan et al. (2012) using convex-hull approximations of the links of the robot.

All videos showing the presented simulations and real experiments are included in the material accompanying this paper⁶.

6.1 Non-Co-Planar Contact Scenario

The NSPG has been tested in the scenario where an external planner returns a series of feasible stances only. In this case, the contact state can be written as

Specifically, the humanoid robot has to climb a stair of three steps on a sequence of 18 stances. The first two steps are flat, while the last two are rotated 0.25 rad along the x-axis (see Figure 5). The stances are such that they respect the principle of connectivity described in Section 4, and the NSPG has to find a series of feasible configurations , each one corresponding to a specific stance , in the hypothesis that a feasible configuration exists for each stance. The sequence of stances includes contacts with both hands to enhance static stability. The NSPG is used after the planner and generates a sequence of configurations starting from a sequence of stances. Following the algorithm described in Section 5, the previous configuration has been used as the nominal configuration for the generation of , starting from a known homing configuration .

FIGURE 5

In this scenario, we want to stress the capability of the NSPG to find collision-free and stable configurations while stepping on non-co-planar stances, using a whole-body approach.

We analyze the NSPG performance on 10 runs using the same sequence of stances. The results are collected in Figure 6: the NSPG is always able to find all the 18 feasible configurations, changing the active links accordingly, in approximately 2.2 s with an average of 0.12 s for each generated configuration.

FIGURE 6

6.2 Corridor Scenario

The NSPG has been also tested in a particularly tricky scenario for a sample-based planner algorithm: a narrow corridor (Kingston et al. (2018)). In particular, CENTAURO and COMAN+ are asked to traverse a narrow corridor wide and high. Taking advantage of the capability of CENTAURO to roll through the next stance instead of taking a step, its active contacts do not change during the whole planning. In this scenario, CENTAURO proves the effectiveness of our method to generate (self-)collision-free postures being the corridor approximately of the same size of the robot, enhancing the collision occurrances as the robot itself.

When planning with COMAN+ instead, locomotion is achieved by continuously switching between the two feet, i.e., the walking pattern. In particular, a sequence of single and double stances is computed by the planner and connectivity is guaranteed generating single-stance statically stable configurations. Indeed, the next double-stance configuration can be reached if and only if it is single-stance stable, avoiding the robot to go through an unstable region while walking (see Section 4). This is particularly challenging from both stability and collision safeness points of view since the robot has to move its CoM between the two stance feet while moving in the corridor.

In this scenario, the NSPG is used inside a planner routine, implemented using OMPL Şucan et al. (2012), to validate the sampled stances. Every time a new stance is sampled, the contact state is added to the search tree if the NSPG has been able to compute a feasible whole-body configuration in the given time .

Three parameter setups were tried in this scenario to test how the performance of the NSPG changes. This was evaluated collecting data about the average time employed to find a feasible posture:with being the time taken by the NSPG in a single call and m being the total NSPG calls. Additionally, the NSPG performance is evaluated considering also its rate of success and the average number of iterations the NSPG takes to find a feasible solution computed aswith , , and being the average time required by the HIK and the stability/collision check, respectively. The integration time is kept fixed as seconds, as well as the parameter , to guarantee small differences between adjacent configurations. The timeout is varied between 0.5, 1, and 2 s. Intuitively, this variation in should guarantee a higher success rate of the algorithm that is allowed to search a feasible configuration for a longer time. On the contrary, when the mean time to find a single feasible posture increases, the timeout also increases.

An additional statistic has been added considering the causes of failure. Indeed, the NSPG can fail due to collision check and static stability check failure. The necessity to always check both the stability and collision safeness is required by the NSPG to update the velocity vector as described in Section 5.4. During the simulations, the number of centroidal statics and collision checks has been collected, and their percentage w.r.t. the total number of fails is shown in Table 2. For CENTAURO, it is observed that the cause of all the failures of the NSPG is (self-)collisions. Furthermore, the quadrupedal structure of CENTAURO guarantees static stability almost in every configuration projected on each stance generated by the planner, and contact states are discarded only when a collision is unavoidable.

Differently, when generating configurations with COMAN+, the stability check fails twice the collision check since we are looking for a single-stance stable configuration while moving through a narrow environment. Notice that the sum of the two percentages goes over 100% since the NSPG can fail due to a contemporary fail of the stability and collision checks.

The results are collected in Table 3, which confirm the observations just done. Screenshots of the simulations with both COMAN+ and CENTAURO are shown in Figure 7 and Figure 8. Real experiments with CENTAURO in this scenario are shown in Figure 9. A complete description of the controller used is given in Appendix 1. In both the simulated and real experiments, the surrounding environment is detected using perception data based on a 3D point cloud generated by a Lidar sensor. The data are collected following the work in Hornung et al. (2013).

FIGURE 7

FIGURE 8

FIGURE 9

6.3 Discussion

The NSPG has been designed to generate whole-body configurations for multi-limbed robots in a particularly complex environment. For instance, (multi-)contact planners generally require a precise representation of an extensive environment, usually obtained through point clouds. As already mentioned, in these scenarios, the use of HIK solvers, coupled with specific constraints for obstacle avoidance, can be computationally heavy since they are based on the computation of the distances between each link of the robot and each obstacle in the environment (i.e., each point of the point cloud). Based on these distances, several methods have been proposed to move the robot away from the singularity. On the contrary, the NSPG does not need any particular strategy to avoid collisions, and the only requirement is the computation of the distances mentioned above. The results in Table 3 show how this computation can be efficiently done even in a complex environment acquired through a Lidar sensor.

To motivate this, an additional simulation with COMAN+ has been run in an environment similar to the one described in Section 6.2. This time, the NSPG does not take into account collisions to generate the random velocity vector , but instead an additional constraint for collision avoidance is added to the HIK solver. The constraint is designed following the work done as in, and the results are collected in Table 4. Simulations were run using the same parameter set as in Section 6.2 setting .

As expected, when using the linear constraint for collision avoidance in a complex environment detected through a dense point cloud, the time to solve the HIK dramatically increases by three orders of magnitude. The average time for the collision check is dropped to zero, but this improvement is not enough to justify such an increase in computational cost for the HIK and this is reflected in the overall performances of the NSPG. The percentage of success drops to 43.7% since the average time to find a feasible configuration goes over 2 s with a timeout of 1 s. In addition, the average number of iterations for each call of the NSPG decreases to 15 reducing its capability to explore the workspace.

7 Conclusion

This work presents a novel algorithm, named the null-space posture generator (NSPG), able to efficiently generate stable and (self-)collision-free whole-body postures for a generic, multi-limbed, floating-base robot, given a sequence of stances. The NSPG has been developed to speed up the whole-body motion planning of complex robotic systems when passing through particularly challenging environments keeping the tuning procedure as light as possible. Indeed, benefits from computation and efficiency points of view have been demonstrated when applying this algorithm in a particularly complex environment compared to previous methods that use the constraint in the HIK to generate statically stable and collision-free configurations. Furthermore, it can also be used independently as a posture generator, given the active contacts as shown in Section 6.1.

Multiple experiments on two profoundly different robotic platforms, COMAN+ and CENTAURO, demonstrated that the NSPG is capable of quickly generating stable and collision-free configurations for a legged robot in contact with the environment, exploiting null-space motions. In particular, CENTAURO represents a challenging platform for planning considering the high number of DoFs. Real experiments were also carried out on CENTAURO using a Lidar sensor to perceive the environment, demonstrating the applicability of the proposed approach to a real scenario.

Our implementation of the NSPG can generate approximately 1,000 configurations per second, guaranteeing a good exploration despite using a light random approach able to adapt while exploiting the robot’s workspace depending on the unfeasibility occurrence.

The proposed method, even if based on a random approach, presents a good level of reliability which is observed in the result obtained in the two considered scenarios. Additionally, it does not require a big effort to tune its parameters, which does not depend on the robotic platform in use, as it has been seen by the general applicability of the algorithm to two profoundly different robotic platforms.

Comparing our results to the recent work proposed in Shigematsu et al. (2019), in the cluttered scenario (Section 6.1), we were able to double the configurations with an average time smaller than three orders of magnitude, guaranteeing minimal differences between adjacent postures.

Future work will involve the use of the NSPG in a multi-contact planner scenario similar to the one used to generate stances in Section 6.1. Furthermore, the stability check could be improved considering centroidal dynamics, allowing higher dynamic motions and enlarging the set of possible feasible configurations and applications, i.e., kinodynamic planning. Additionally, post-processing of the joint trajectory generated should be investigated in order to correct any unfeasibility during the interpolation or to minimize a user-defined cost function (i.e., minimum length path), using the planner output as an initial guess.

References

• 1

  BouyarmaneK.KheddarA. (2012). Humanoid Robot Locomotion and Manipulation Step Planning. Adv. Robotics26, 1099–1126. 10.1080/01691864.2012.686345

  • CrossRef
  • Google Scholar
  • View reference in article

• 2

  BuchananR.BandyopadhyayT.BjelonicM.WellhausenL.HutterM.KottegeN. (2019). Walking Posture Adaptation for Legged Robot Navigation in Confined Spaces. IEEE Robot. Autom. Lett.4, 2148–2155. 10.1109/lra.2019.2899664

  • CrossRef
  • Google Scholar
  • View reference in article

• 3

  CaronS.PhamQ.-C.NakamuraY. (2015). “Stability of Surface Contacts for Humanoid Robots: Closed-form Formulae of the Contact Wrench Cone for Rectangular Support Areas,” in IEEE International Conference on Robotics and Automation (ICRA), 26-30 May 2015, Seattle, WA, USA, 5107–5112. 10.1109/ICRA.2015.7139910

  • CrossRef
  • Google Scholar
  • View reference in article

• 4

  CognettiM.MohammadiP.OrioloG. (2015). “Whole-body Motion Planning for Humanoids Based on Com Movement Primitives,” in IEEE-RAS International Conference on Humanoid Robots (HUMANOIDS), 3-5 November 2015, Seoul, Korea (South), 1090–1095. 10.1109/humanoids.2015.7363504

  • CrossRef
  • Google Scholar
  • View reference in article

• 5

  DeitsR.TedrakeR. (2014). “Footstep Planning on Uneven Terrain with Mixed-Integer Convex Optimization,” in 2014 IEEE-RAS International Conference on Humanoid Robots (HUMANOIDS), 18-20 Nov. 2014, Madrid, Spain, 279–286. 10.1109/HUMANOIDS.2014.7041373

  • CrossRef
  • Google Scholar
  • View reference in article

• 6

  EscandeA.KheddarA.MiossecS. (2013). Planning Contact Points for Humanoid Robots. Robotics Autonomous Syst.61, 428–442. 10.1016/j.robot.2013.01.008

  • CrossRef
  • Google Scholar
  • View reference in article

• 7

  EscandeA.MansardN.WieberP.-B. (2014). Hierarchical Quadratic Programming: Fast Online Humanoid-Robot Motion Generation. Int. J. Robotics Res.33, 1006–1028. 10.1177/0278364914521306

  • CrossRef
  • Google Scholar
  • View reference in article

• 8

  FangC.RocchiA.HoffmanE. M.TsagarakisN. G.CaldwellD. G. (2015). “Efficient Self- Collision Avoidance Based on Focus of Interest for Humanoid Robots,” in 2015 IEEE-RAS 15th International Conference on Humanoid Robots (HUMANOIDS), 3-5 November 2015, Seoul, Korea (South), 1060–1066. 10.1109/HUMANOIDS.2015.7363500

  • CrossRef
  • Google Scholar
  • View reference in article

• 9

  FerrariP.CognettiM.OrioloG. (2018). “Anytime Whole-Body Planning/replanning for Humanoid Robots,” in IEEE-RAS International Conference on Humanoid Robots (Humanoids), 6-9 November 2018, Beijing, China, 1–9. 10.1109/humanoids.2018.8624935

  • CrossRef
  • Google Scholar
  • View reference in article

• 10

  FerreauH. J.KirchesC.PotschkaA.BockH. G.DiehlM. (2014). qpOASES: a Parametric Active-Set Algorithm for Quadratic Programming. Math. Prog. Comp.6, 327–363. 10.1007/s12532-014-0071-1

  • CrossRef
  • Google Scholar

• 11

  FerrolhoH.MerktW.YangY.IvanV.VijayakumarS. (2018). “Whole-body End-Pose Planning for Legged Robots on Inclined Support Surfaces in Complex Environments,” in 2018 IEEE-RAS 18th International Conference on Humanoid Robots (HUMANOIDS), 6-9 Nov. 2018, Beijing, China, 944–951. 10.1109/HUMANOIDS.2018.8625026

  • CrossRef
  • Google Scholar

• 12

  GutmannJ.-S.FukuchiM.FujitaM. (2005). “Real-time Path Planning for Humanoid Robot Navigation,” in International Joint Conferences on Artificial Intelligence (IJCAI), 30 July 2005- 5 August 2005, Edinburgh Scotland, 1232–1237.

  • Google Scholar

• 13

  HauserK.BretlT.LatombeJ.-C.HaradaK.WilcoxB. (2008). Motion Planning for Legged Robots on Varied Terrain. Int. J. Robotics Res.27, 1325–1349. 10.1177/0278364908098447

  • CrossRef
  • Google Scholar

• 14

  HauserK.BretlT.LatombeJ. (2005). “Non-gaited Humanoid Locomotion Planning,” in IEEE-RAS International Conference on Humanoid Robots (HUMANOIDS), 5-5 Dec. 2005, 5-5 Dec. 2005, Tsukuba, Japan, 7–12.

  • Google Scholar

• 15

  HoffmanE. M.RocchiA.LaurenziA.TsagarakisN. G. (2017). “Robot Control for Dummies: Insights and Examples Using Opensot,” in 2017 IEEE-RAS 17th International Conference on Humanoid Robotics (HUMANOIDS), 15-17 Nov. 2017, Birmingham, UK, 736–741. 10.1109/HUMANOIDS.2017.8246954

  • CrossRef
  • Google Scholar

• 16

  HornungA.WurmK. M.BennewitzM.StachnissC.BurgardW. (2013). OctoMap: An Efficient Probabilistic 3D Mapping Framework Based on Octrees. Autonomous Robots34, 189–206. 10.1007/s10514-012-9321-0

  • CrossRef
  • Google Scholar

• 17

  KanounO.LamirauxF.WieberP.-B.KanehiroF.YoshidaE.LaumondJ.-P. (2009). “Prioritizing Linear equality and Inequality Systems: Application to Local Motion Planning for Redundant Robots,” in IEEE international conference on robotics and automation (ICRA), 12-17 May 2009, Kobe, Japan, 2939–2944. 10.1109/robot.2009.5152293I

  • CrossRef
  • Google Scholar

• 18

  KanounO.LamirauxF.WieberP.-B. (2011). Kinematic Control of Redundant Manipulators: Generalizing the Task-Priority Framework to Inequality Task. IEEE Trans. Robot.27, 785–792. 10.1109/tro.2011.2142450

  • CrossRef
  • Google Scholar

• 19

  KashiriN.CordascoS.GuriaP.MarganA.TsagarakisN. G.BaccelliereL.et al (2019). Centauro: A Hybrid Locomotion and High Power Resilient Manipulation Platform. IEEE Robot. Autom. Lett.4, 1595–1602. 10.1109/lra.2019.2896758

  • CrossRef
  • Google Scholar

• 20

  KingstonZ.MollM.KavrakiL. E. (2018). Sampling-based Methods for Motion Planning with Constraints. Annu. Rev. Control. Robot. Auton. Syst.1, 159–185. 10.1146/annurev-control-060117-105226

  • CrossRef
  • Google Scholar

• 21

  KuffnerJ. J.NishiwakiK.KagamiS.InabaM.InoueH. (2001). “Footstep Planning Among Obstacles for Biped Robots,” in IEEE-RSJ International Conference on Intelligent Robots and Systems (IROS), 29 Oct.-3 Nov. 2001, Maui, HI, 1, 500–505.

  • Google Scholar

• 22

  KuindersmaS.DeitsR.FallonM.ValenzuelaA.DaiH.PermenterF.et al (2016). Optimization-based Locomotion Planning, Estimation, and Control Design for the Atlas Humanoid Robot. Auton. Robot40, 429–455. 10.1007/s10514-015-9479-3

  • CrossRef
  • Google Scholar

• 23

  LaurenziA.HoffmanE. M.MuratoreL.TsagarakisN. G. (2019). “CartesI/O: A Ros Based Real-Time Capable Cartesian Control Framework,” in 2019 International Conference on Robotics and Automation (ICRA), 20-24 May 2019, Montreal, QC, 591–596. 10.1109/ICRA.2019.8794464

  • CrossRef
  • Google Scholar

• 24

  Mingo HoffmanE.TsagarakisN. G. (2021). The Math of Tasks: a Domain Specific Language for Constraint-Based Task Specification. Int. J. Humanoid Robotics, 2150008. under review. 10.1142/s0219843621500080

  • CrossRef
  • Google Scholar

• 25

  PanJ.ChittaS.ManochaD. (2012). “Fcl: A General Purpose Library for Collision and Proximity Queries,” in IEEE International Conference on Robotics and Automation (ICRA), 3859–3866. 10.1109/icra.2012.6225337

  • CrossRef
  • Google Scholar

• 26

  PolveriniM. P.LaurenziA.HoffmanE. M.RuscelliF.TsagarakisN. G. (2020). Multi-Contact Heavy Object Pushing with a Centaur-type Humanoid Robot: Planning and Control for a Real Demonstrator. IEEE Robot. Autom. Lett.5, 859–866. 10.1109/lra.2020.2965906

  • CrossRef
  • Google Scholar

• 27

  RatliffN.ZuckerM.BagnellJ. A.SrinivasaS. (2009). Chomp: Gradient Optimization Techniques for Efficient Motion Planning. IEEE International Conference on Robotics and Automation (ICRA), 12-17 May 2009, Kobe, Japan, 489–494. 10.1109/ROBOT.2009.5152817

  • CrossRef
  • Google Scholar

• 28

  RuscelliF.PolveriniM. P.LaurenziA.HoffmanE. M.TsagarakisN. G. (2020). “A Multi-Contact Motion Planning and Control Strategy for Physical Interaction Tasks Using a Humanoid Robot,” in IEEE - RSJ International Conference on Intelligent Robots and Systems (IROS), 24 Oct.-24 Jan. 2021, 24 Oct.-24 Jan. 2021, 3869–3876. 10.1109/IROS45743.2020.9340745

  • CrossRef
  • Google Scholar

• 29

  ShigematsuR.MurookaM.KakiuchiY.OkadaK.InabaM. (2019). “Generating a Key Pose Sequence Based on Kinematics and Statics Optimization for Manipulating a Heavy Object by a Humanoid Robot,” in 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 3-8 November 2019, Macau, China, 3852–3859. 10.1109/IROS40897.2019.8967902

  • CrossRef
  • Google Scholar

• 30

  StasseO.EscandeA.MansardN.MiossecS.EvrardP.KheddarA. (2008). “Real-time (Self)-collision Avoidance Task on a Hrp-2 Humanoid Robot,” in IEEE International Conference on Robotics and Automation (ICRA). 19-23 May 2008, Pasadena, CA, 3200–3205. 10.1109/ROBOT.2008.4543698

  • CrossRef
  • Google Scholar

• 31

  StellatoB.BanjacG.GoulartP.BemporadA.BoydS. (2020). OSQP: an Operator Splitting Solver for Quadratic Programs. Math. Prog. Comp.12, 637–672. 10.1007/s12532-020-00179-2

  • CrossRef
  • Google Scholar

• 32

  SucanI. A.MollM.KavrakiL. E. (2012). The Open Motion Planning Library. IEEE Robot. Automat. Mag.19, 72–82. 10.1109/mra.2012.2205651

  • CrossRef
  • Google Scholar

• 33

  TonneauS.Del PreteA.PettréJ.ParkC.ManochaD.MansardN. (2018). An Efficient Acyclic Contact Planner for Multiped Robots. IEEE Trans. Robot.34, 586–601. 10.1109/TRO.2018.2819658

  • CrossRef
  • Google Scholar

• 34

  YangY.IvanV.MerktW.VijayakumarS. (2016). “Scaling Sampling-Based Motion Planning to Humanoid Robots,” in IEEE International Conference on Robotics and Biomimetics (ROBIO). 3-7 December 2016, Qingdao, China, 1448–1454. 10.1109/ROBIO.2016.7866531

  • CrossRef
  • Google Scholar

• 35

  YangY.MerktW.FerrolhoH.IvanV.VijayakumarS. (2017). Efficient Humanoid Motion Planning on Uneven Terrain Using Paired Forward-Inverse Dynamic Reachability Maps. IEEE Robot. Autom. Lett.2, 2279–2286. 10.1109/LRA.2017.2727538

  • CrossRef
  • Google Scholar