edinburgh research explorer · assume the quasi–static balance constraint and joint limits...

Edinburgh Research Explorer

Scaling Sampling-based Motion Planning to Humanoid Robots

Citation for published version:Yang, Y, Ivan, V, Merkt, W & Vijayakumar, S 2017, Scaling Sampling-based Motion Planning to HumanoidRobots. in IEEE International Conference on Robotics and Biomimetics ROBIO 2016. Institute of Electricaland Electronics Engineers (IEEE), pp. 1448-1454, IEEE INTERNATIONAL CONFERENCE ON ROBOTICSAND BIOMIMETICS 2016, Qingdao, China, 3/12/16. https://doi.org/10.1109/ROBIO.2016.7866531

Digital Object Identifier (DOI):10.1109/ROBIO.2016.7866531

Link:Link to publication record in Edinburgh Research Explorer

Document Version:Peer reviewed version

Published In:IEEE International Conference on Robotics and Biomimetics ROBIO 2016

General rightsCopyright for the publications made accessible via the Edinburgh Research Explorer is retained by the author(s)and / or other copyright owners and it is a condition of accessing these publications that users recognise andabide by the legal requirements associated with these rights.

Take down policyThe University of Edinburgh has made every reasonable effort to ensure that Edinburgh Research Explorercontent complies with UK legislation. If you believe that the public display of this file breaches copyright pleasecontact [email protected] providing details, and we will remove access to the work immediately andinvestigate your claim.

Download date: 08. Jun. 2021

https://doi.org/10.1109/ROBIO.2016.7866531https://doi.org/10.1109/ROBIO.2016.7866531https://www.research.ed.ac.uk/portal/en/publications/scaling-samplingbased-motion-planning-to-humanoid-robots(413b2812-21e4-4d73-9a78-97e400e00e08).html

Scaling Sampling–based Motion Planning to Humanoid Robots

Yiming Yang, Vladimir Ivan, Wolfgang Merkt, Sethu Vijayakumar

Abstract— Planning balanced and collision–free motion forhumanoid robots is non–trivial, especially when they are oper-ated in complex environments, such as reaching targets behindobstacles or through narrow passages. We propose a methodthat allows us to apply existing sampling–based algorithmsto plan trajectories for humanoids by utilizing a customizedstate space representation, biased sampling strategies, and asteering function based on a robust inverse kinematics solver.Our approach requires no prior offline computation, thus onecan easily transfer the work to new robot platforms. We testedthe proposed method solving practical reaching tasks on a 38degrees–of–freedom humanoid robot, NASA Valkyrie, showingthat our method is able to generate valid motion plans thatcan be executed on advanced full–size humanoid robots. Wealso present a benchmark between different motion planningalgorithms evaluated on a variety of reaching motion problems.This allows us to find suitable algorithms for solving humanoidmotion planning problems, and to identify the limitations ofthese algorithms.

I. INTRODUCTION

Humanoid robots are highly redundant systems that aredesigned for accomplishing a variety of tasks in environ-ments designed for human. However, humanoids have a largenumber of degrees–of–freedom (DoF) which makes motionplanning extremely challenging. In general, optimization–based algorithms are suitable for searching for optimalsolutions even in high dimensional systems [1] [2], but itis non–trivial to generate optimal collision–free trajectoriesfor humanoids using optimization approaches, especially incomplex environments. This is mainly due to the highly non–linear map between the robot and the collision environment.This mapping can be modelled in some abstract spaces toprovide real–time collision avoidance capabilities on lowDoF robotic arms [3] difficult for high DoF humanoidsdue to the curse of dimensionality and it often causeslocal minima problems. Additionally, solving locomotionand whole–body manipulation in complex environments asone combined problem requires searching through a largespace of possible actions. Instead, it is more effective tofirst generate robust walking plans to move the robot to adesired standing location, and then generate collision–freemotion with stationary feet [4]. Although assuming fixedfeet position may be viewed as restrictive, we argue thata large variety of whole body manipulation tasks can stillbe executed as a series of locomotion and manipulationsubtasks. We propose an extension to a family of samplingbased motion planning algorithms that will allow us to plan

All authors are with School of Informatics, University of Edinburgh(Informatics Forum, 10 Crichton Street, Edinburgh, EH8 9AB, UnitedKingdom). email: [email protected]

Fig. 1: Collision–free and balanced whole–body motionexecuted on the 38 DoF NASA Valkyrie robot.

collision–free whole–body motions on floating based systemswhich require active balancing.

Sampling–based planning (SBP) algorithms, such as RRT[5] and PRM [6], are capable of efficiently generating glob-ally valid collision-free trajectories due to their simplicity.In the past two decades, SBP algorithms have been appliedto countless problems with a variety of derivatives, suchas RRT-Connect [7], Expansive Space Trees (EST, [8]),RRT*/PRM* [9], Kinematic Planning by Interior-ExteriorCell Exploration (KPIECE, [10]), and many others [11].However, since the SBP algorithms were originally designedfor mobile robots and low DoF robotic arms, using themon high DoF systems requiring active balancing is stillchallenging. We will call a robot pose statically balancedif the controller can achieve an equilibrium in this statewhile achieving zero velocity and acceleration (e.g. whenthe projection of centre of mass lies within the support poly-gon). The subset of robot configurations with this propertyforms a low dimensional manifold defined by the balanceconstraint. In practice, the rejection rate of random samplesis prohibitively high without the explicit or implicit knowl-edge of the manifold. Approaches have been proposed toaddress this particular problem of using SBP algorithms forhumanoid robots. Kuffner at al. [12] use a heavily customizedRRT–Connect algorithm to plan whole body motion forhumanoids, where they only sample from a pre-calculated

arX

iv:1

607.

0747

0v2

[cs

.RO

] 2

9 Ju

l 201

6

pool of postures for which the robot is in balance. Hauserat al. [13] introduce motion primitives into SBP algorithmswhere the sampler only samples states around a set ofpre–stored motion primitives. A similar approach is usedin [14] with centre–of–mass (CoM) movement primitives.These approaches share the common idea of using an offlinegenerated sample set to bootstrap online processes, thus al-lowing algorithms to bypass the expensive online generationof balanced samples. However, this leads to the problemwhere one has to store a significant number of samples todensely cover the balance manifold, otherwise the algorithmsmay fail while valid solutions exist but were not stored duringoffline processing. Another issue is that the pre–processing isnormally platform specific, which makes it difficult and timeconsuming to transfer the work to other robot platforms.

To this end, instead of developing new SBP algorithmsspecifically for humanoids, we focus on enabling the stan-dard SBP algorithms to solve humanoids motion planningproblems by modifying the underlying key components ofgeneric SBP approaches, such as space representation, sam-pling strategies and interpolation functions. In order to makethe method generic for any humanoid platforms, rather thanstore balanced samples during offline processing, we usea non–linear optimization based [15] whole–body inversekinematics (IK) solver to generate balanced samples on–the–fly. Thus, the proposed method can be easily applied todifferent humanoid robot platforms without extensive pre–processing and setup. We evaluate the proposed method ona 36 DoF Boston Dynamics Atlas and a 38 DoF NASAValkyrie humanoid robots, to show that our method is capa-ble of generating reliable collision–free whole–body motionfor a generic humanoid. We also evaluate the differencebetween sampling in end–effector and configuration spacesfor different scenarios, and compare the planning time andtrajectory length to find an optimal trade off between ef-ficiency and optimality. In particular, we apply our workto solve practical reaching tasks on the Valkyrie robot, ashighlighted in Fig. 1, showing that the proposed method cangenerate reliable whole–body motion that can be executedon full–size humanoid robots.

II. PROBLEM FORMULATION

Let C ∈ RN+6 be a robot’s configuration space with Nthe number of articulated joints and the additional 6-DoFof the under actuated virtual joint that connects the robot’spelvis (Tpelvis ) and the world W ∈ SE(3). Let q ∈ C bethe robot configuration state, Cbalance ⊂ C the manifold thatcontains statically balanced configurations, Cfree ⊂ C themanifold contains collision free configurations and Cvalid ≡Cbalance ∩ Cfree the valid configuration manifold.

For humanoid robots, valid trajectories can only containstates from valid configuration manifold, i.e. q[0:T ] ⊂ Cvalid .Generating collision free samples is straightforward by usingrandom sample generators and standard collision checkinglibraries. However, generating balanced samples is non-trivial, where a random sampling technique is incapable

of efficiently finding balanced samples on the low dimen-sional manifold Cbalance by sampling in high dimensionalconfiguration space C. Guided sampling or pre–samplingprocess is required for efficient valid sample generation.In our approach, a whole-body inverse kinematic solveris employed to produce statically balanced samples. Staticbalance constraint is a combination of feet and CoM posesconstraints, i.e. the static balance constraint is considered assatisfied when the robot’s feet has stable contacts with groundand the CoM ground projection stays within the supportpolygon.

A. Whole–body Inverse KinematicsGiven a seed configuration qseed and nominal config-

uration qnominal and a set of constraints C, an outputconfiguration that satisfies all the constraints can be generallyformulated as:

q∗ = IK (qseed ,qnominal ,C) (1)

The Constraint set for a whole–body humanoid robot mayinclude single joint constraints, such as position and velocitylimits for articulated joints, it may also include workspacepose constraints, e.g. end–effector poses, centre-of-mass po-sition. In the rest of the paper, unless specified otherwise, weassume the quasi–static balance constraint and joint limitsconstraints are included in C by default. We formulate theIK problem as a non-linear optimization problem (NLP) ofform:

q∗ = arg minq∈RN+6 ‖q− qnominal‖2Qq (2)subject to bl ≤ q ≤ bu (3)

ci(q) ≤ 0, ci ∈ C (4)where Qq � 0 is the weighting matrix, bl and bu are thelower and upper joint bounds. We use a randomly sampledstate as the seed pose qseed . We then use this pose as theinitial value in the first iteration of SQP solver. Dependingon the implementation of the SBP algorithm, we eitherchoose qnominal to be the current robot state or one of theneighbouring poses drawn from the pool of candidate posesalready explored by the SBP algorithm.

B. Sampling–based Motion PlanningLet x ∈ X be the space where the sampling is carried out.

The planning problem can be formulated as

q[0:T ] = Planning(Rob,Env,x0,xT ) (5)

where Rob is the robot model and Env is the environmentinstance in which this planning problem is defined. x0 andxT are the initial and desired states.

In order for SBP algorithms to be able to plan motionsfor humanoid robots, we need to modify the followingcomponents that are involved in most algorithms as shownin Fig. 2: the space X where the sampling is carried out;the strategies to draw random samples; and the interpolationfunction which is normally used in steering and motionevaluation steps. In the next section, we will discuss thedetails of modifications we applied on those components forscaling standard SBP algorithms to humanoids.

𝒙"#$%#𝒙&'$%'"#

𝒙$

𝒙( 𝒙)*$+

𝒙%$&,*-

𝒙&'.

Motion evaluation

Interpolation

Sampling

Samplingspace𝒳

Fig. 2: Instead of developing new algorithms, we mod-ify those underlying components in SBP solvers to makestandard algorithms be capable of solving motion planningproblems for humanoid robots.

III. SAMPLING–BASED PLANNING FOR HUMANOIDS

We separate the work into two parts, configuration spacesampling and end–effector space sampling. In configurationspace sampling approach, the state is represented in RN+6space with joint limits and maximum allowed base movementas the bounds, the sampling state is identical to robot config-uration, i.e. x = q ∈ C. For reaching and grasping problems,one might be interested in biasing the sampling in the end–effector related constraints, e.g. to encourage shorter end–effector traverse distance. The end–effector space approachsamples in SE(3) space with a region of interests aroundthe robot as the bounds, the state is equivalent to the end–effector’s forward kinematics, i.e. x = Φ(q) ∈ W whereΦ(·) is the forward kinematics mapping. However, the finaltrajectories are represented in configuration space, thus weassociate a corresponding configuration for each end–effectorspace state to avoid ambiguity and duplicated calls of IKsolver.

A. Configuration Space Sampling

Algorithm 1 highlights the components’ modificationsrequired for sampling in configuration space:

1) Sampling Strategies: For sampleUniform(), we firstgenerate random samples from X and then use fullbody IKsolver to process the random samples to generate samplesfrom the balanced manifold Xbalance

qrand = IK (q̄rand , q̄rand ,C) (6)

where q̄rand ∈ X is a uniform random configuration andqrand ∈ Xbalance is random sample from the balancedmanifold. We use q̄rand as nominal pose since we wantto generate random postures rather than postures closeto other already existing samples. This is to indirectlyencourage exploration of the null-space of the task. Theconstraint set C contains static balance constraint and jointlimits constraints. When sampling around a given state,sampleUniformNear(qnear , d), we first get a random stateq̄rand that is close to qnear within distance d. The IK solveris invoked with q̄rand as the seed pose, and qnear as the

Algorithm 1 Humanoid Configuration Space SBPsampleUniform()

1: succeed = False2: while not succeed do3: q̄rand = RandomConfiguration()4: qrand , succeed = IK (q̄rand , q̄rand ,C)

return qrand

sampleUniformNear(qnear , d)1: succeed = False2: while not succeed do3: A← Zeros(N + 6)4: while not succeed do5: q̄rand = RandomNear(qnear , d)6: Set constraint ‖qrand − q̄rand‖W < A7: qrand , succeed = IK (q̄rand ,qnear ,C)8: Increase A9: if distance(qrand ,qnear ) > d then

10: succeed = Falsereturn qrand

interpolate(qa,qb, d)1: q̄int = InterpolateConfigurationSpace(qa,qb, d)2: succeed = False3: A← Zeros(N + 6)4: while not succeed do5: Set constraint ‖qint − q̄int‖W < A6: qint , succeed = IK (q̄int ,qa,C)7: Increase A

return qint

nominal pose. An additional configuration space constraintis added to the constraint set

‖qrand − q̄rand‖W ≤ A (7)

where A ∈ RN+6 is a tolerance vector initially set tozero. In most cases the system will be over constrained,in which case we need to increase the tolerance to ensurebalance. Normally, the lower–body joints are neglected first,i.e. increasing corresponding wi, meaning that we allow thelower–body joints to deviate from q̄rand in order to keepfeet on the ground and maintain balance. We use xnear asthe nominal pose since later on the random state is likely tobe appended to qnear , where one wants the random state beclose to the near state. The new sample is discarded if thedistance between qnear and qrand exceeds the limit d.

2) Interpolation: In order to find a balanced state interpo-lated along two balanced end–point states, we first find theinterpolated, likely to be un–balanced state

q̄int = qa + d‖qb − qa‖. (8)

A similar configuration space constraint to (7) is applied toconstrain the balanced interpolated state qint close to q̄int

|qint − q̄int‖ ≤ A (9)

Algorithm 2 Humanoid End–Effector Space SBPsampleUniform()

1: succeed = False2: while not succeed do3: x̄rand = RandomSE3 ()4: Set constraint ‖x̄rand − Φ(qrand)‖ ≤ 05: qrand , succeed = IK (q̄rand , q̄rand ,C)

6: xrand = Φ(qrand)return xrand ,qrand

sampleUniformNear(xnear , d)1: succeed = False2: while not succeed do3: x̄rand = RandomNearSE3 (xnear , d)4: Set constraint ‖x̄rand − Φ(qrand)‖ ≤ 05: qrand , succeed = IK (qrand ,qnear ,C)

6: xrand = x̄randreturn xrand ,qrand

interpolate(xa,xb, d)1: x̄int = InterpolateSE3 (xa,xb, d)2: succeed = False3: B ← Zeros(SE3)4: while not succeed do5: Set constraint ‖x̄int − Φ(qint)‖ < B6: qint , succeed = IK (qa,qa,C)7: Increase B8: xint = Φ(qint)

return xint ,qint

The two end-point states qa and qb are valid samplesgenerated using our sampling strategies. Due to the convexformulation of the balance constraint, a valid interpolatedstate is guaranteed to be found. It is worth mentioning thatin some cases the interpolation distance equation no longerholds after increasing the tolerance, i.e. ‖xint−xa‖‖xb−xa‖ 6= d.However, this is a necessary step to ensure that the balanceconstraint are satisfied.

B. End-Effector Space Sampling

Algorithm 2 highlights the components’ modificationsrequired for sampling in end–effector space:

1) Sampling Strategies: It is straight forward to sample inSE(3) space, however, it is non–trivial to sample balancedsamples from the Xbalance manifold. For sampleUniform(),we first randomly generate SE(3) state x̄rand within a regionof interest in front of the robot. The whole–body IK isinvoked with an additional end–effector pose constraint

‖x̄rand − Φ(qrand)‖ ≤ 0 (10)

The sampler keeps drawing new random states x̄rand untilthe SQP solver returns a valid output q∗. The valid ran-dom state xrand can be calculated using forward kinemat-ics, e.g. xrand = Φ (q∗). The same procedure applies to

TABLE I: Planning time of empty space reaching problemcrossing different algorithms, in seconds.

Algorithms Sampling SpaceEnd–Effector Space Configuration SpaceRRT 25.863± 22.894 1.4129± 1.4466PRM 4.2606± 3.0322 0.5912± 0.5912EST 28.055± 18.270 0.3112± 0.3112BKPIECE 5.3989± 5.9470 0.1781± 0.0332SBL 3.0602± 0.9859 0.2804± 0.0480RRT–Connect 2.8228± 0.3412 0.1853± 0.0450

sampleNear(xnear , d), but using xnear as the seed configu-ration.

2) Interpolation: Similar to sampling near a given state,for interpolation in end–effector space, we first find theinterpolated state x̄int ∈ SE(3) and add the following terminto constraint set

‖x̄int − Φ(q)‖ ≤ B (11)

where B ∈ R6 is a tolerance vector initially set to zero.If the system is over constrained after adding end–effectorpose constraint, we need to selectively relax the tolerancefor different dimensions (x, y, z, roll, pitch, yaw) until theIK solver succeeds. Then we reassign the interpolated stateusing forward kinematics, xint = Φ(qint).

3) Multi-Endeffector Motion Planning: Some tasks re-quire coordinated motion involving multiple end–effectors,e.g. bi–manual manipulation and multi–contact motion. Itis obvious that, from a configuration space point of view,there is no difference as long as the desired configurationis specified. It is also possible for end–effector space sam-pling approach to plan motion with multiple end–effectorconstraints. Let y∗k ∈ SE(3) be the desired pose constraintsfor end–effector k ∈ {1, . . . ,K}. A meta end–effectorspace X ∈ R6×K can be constructed to represent thesampling space for all end–effectors. Similar sampling andinterpolation functions can be implemented by constructingextra constraints for each end–effector k.

IV. EVALUATIONWe aim to generalize the common components of

sampling–based motion planning algorithms for humanoidrobots so that existing algorithms can be used withoutextra modification. We implemented our approach in theEXOTica motion planning and optimization framework [16]as humanoid motion planning solver, which internally in-vokes the SBP planners from the Open Motion PlanningLibrary (OMPL, [17]). We have set up the system withour customized components, and evaluated our approach onthe following 6 representative algorithms: RRT [5], RRT-Connect [7], PRM [6], BKPIECE [10], EST [8]) and SBL[18]. The evaluations are performed on a single thread of the4.0 GHz Intel Core i7-6700K CPU.

A. Empty Space Reaching

In the first experiment, we have the robot reach a targetpose in front of the robot in free space, where only self–collision and balance constraints are considered. This is a

TABLE II: Evaluation of whole–body collision–free motion planning. RRT–Connecte sampling in end–effector space, allother methods sampling in configuration space. C cost is the configuration space trajectory length,W cost is the end–effectortraverse distance in workspace, CoM cost is the CoM traverse distance in workspace. No. evaluation shows the number ofstate evaluation calls, i.e. evaluate if a sampled/interpolated state is valid. No. IK indicates the number of online whole–bodyIK calls, and IK time is the total time required for solving those IK, which is the most time consuming element. The resultis averaged over 100 trails.

Tasks Algorithms Planning time (s) C cost (rad.) W cost (m) CoM cost (m) No. evaluation No. IK IK time (s)

Task 1

BKPIECEc 42.5 ± 26.4 7.37 ± 2.43 2.10 ± 0.80 0.24 ± 0.10 1946 ± 1207 2598 ± 1582 41.4 ± 25.7SBLc 27.8 ± 8.59 6.25 ± 1.06 2.14 ± 0.71 0.23 ± 0.06 1313 ± 418 1508 ± 445 27.0 ± 8.33RRT–Connecte 9.91 ± 4.80 2.93 ± 0.96 0.58 ± 0.11 0.07 ± 0.02 597 ± 354 727 ± 387 9.51 ± 4.58RRT–Connectc 1.53 ± 0.80 2.71 ± 0.68 0.99 ± 0.23 0.11 ± 0.03 95 ± 54 118 ± 64 1.48 ± 0.77

Task 2


Task 3


Fig. 3: Evaluation tasks, from left to right: task 1, targetclose to robot; task 2, target far away from robot; and task3, target behind bar obstacle.

sanity check to show that the proposed method can be usedrobustly across different planning algorithms to generatetrajectories for humanoid robots. We solve the reachingproblem using the 6 testing algorithms in two differentsampling spaces, each across 100 trials. The results areshown in Table I. Although the planning time varies acrossdifferent algorithms and sampling spaces, the result showsthat standard planning algorithms are able to generate motionplans for humanoid robots using our method. However, asexpected, bi–directional algorithms are more efficient thantheir unidirectional variants. Also, sampling in configurationspace is much more efficient than in end–effector space dueto the higher number of IK calls.

B. Collision–free Reaching

We setup three different scenarios, from easy to hard, asillustrated in Fig. 3, to evaluate the performance of differentalgorithms in different sampling spaces. Unfortunately, theevaluation suggests that standard unidirectional algorithmsare unable to solve these problems (within a time limitof 100 seconds). Without bi–directional search, the highdimensional humanoid configuration space is too complexfor sampling–based methods to explore. Table II highlightsthe results using four different bidirectional approaches.Note that when sampling in end–effector space, only RRT–

(a) Trajectories generated using configuration space sampling.

(b) Trajectories generated using end–effector space sampling.

Fig. 4: Whole–body motion plans generated using differentsampling spaces. The task is identical for each column. Ingeneral, configuration space sampling leads to shorter tra-jectory length; end–effector space sampling leads to shorterend–effector traverse distance.

Connect is able to find a valid solution in the given time,other bidirectional search algorithms like BKPIECE andSBL are also unable to find valid trajectories. The resultindicates that RRT–Connect sampling in configuration spaceis the most efficient and the most robust approach forsolving humanoid whole–body motion planning problems.It requires the least exploration, thus bypassing expensiveonline IK queries. Algorithms like BKPIECE and SBL uselow–dimensional projections to bias the sampling, however,the default projections which are tuned for mobile robotsand robotic arms do not scale up to high DoF humanoidrobots, which leads to long planning time and trajectories

(a) Reaching motion on the NASA Valkyrie robot. (b) Reaching motion on the Boston Dynamics Atlas robot.

Fig. 5: Collision–free whole–body motion generated in different scenarios with different robot models. The correspondingCoM trajectories are illustrated in the second row (red dots). The framework is setup so that one can easily switch to newrobot platforms without extensive preparing procedures.

with high costs. This can be improved by better projectionbias, but it is non–trivial to find a suitable bias without finetuning. Also, the trajectories generated using RRT–Connectare shorter, meaning that the motion is more stable androbust. It is worth mentioning that RRT–Connect takes longertime to plan when sampling in the end–effector space than itdoes in the configuration space, but the planned trajectorieshave shorter end–effector and CoM traverse distances. Insome scenarios where planning time is not critical, onechoose to use RRT–Connect in end–effector space to generatetrajectories with shorter end–effector traverse distance. Theseresults also suggest that the whole–body IK computationdominates the planning time. This is in contrast with classicalSBP problems where collision–detection is the the most timeconsuming component. However, the IK solver is necessaryfor keeping balance, as shown in Fig. 5, where the trajecto-ries’ CoM projections are within the support polygon.

In more complex scenarios, such as reaching throughnarrow passages and bi–manual tasks, most algorithms failto generate valid trajectories apart from RRT–Connect. Asmentioned, some algorithms’ performance depends on thebiasing methods, e.g. projection bias and sampling bias.However, it is non–trivial to find the appropriate bias forhumanoids that would generalize across different tasks.Fig. 5 highlights some examples of reaching motion in morecomplex scenarios with different robot models. As statedearlier, this work focuses on generalising SBP algorithmsfor humanoids, where as one can easily setup the systemon new robot platforms. For instance, one can easily switchfrom Valkyrie (Fig. 5a) to Atlas (Fig. 5b) in minutes withoutextensive pre–processing and setup procedures.

In order to test the reliability and robustness of theproposed method, we applied our work on the Valkyrie robotaccomplishing reaching and grasping tasks in different sce-narios, as highlighted in Fig 6. During practical experiments,the collision environment is sensed by the on–board sensorand represented as an octomap [19]. The experiment resultsshow that our method is able to generate collision–freewhole–body motion plans that can be executed on full–size

humanoid robot to realise practical tasks such as reaching andgrasping. A supplementary video of the experiment resultscan be found at https://youtu.be/W48miMKWnW4.

V. CONCLUSION

In this paper we generalise the key components requiredby sampling–based algorithms for generating collision–freeand balanced whole–body trajectories for humanoid robots.We show that by using the proposed methods, standard algo-rithms can be invoked to directly plan for humanoid robots.We also evaluate the performance of different algorithmson solving planning problems for humanoids, and point outthe limitations of some algorithms. A variety of differentscenarios are tested showing that the proposed method cangenerate reliable motion for humanoid robots in differentenvironments. This work can be transferred to differenthumanoid robot models with easy setup procedure that canbe done in very a short period of time, without extensive pre-computation for adapting the existing algorithms to differentrobot models, as we have tested on the 36 DoF BostonDynamics Atlas and the 38 DoF NASA Valkyrie robots. Inparticular, we applied this work on the Valkyrie robot accom-plishing different tasks, showing that the proposed methodcan generate robust collision–free whole–body motion thatcan be executed on real robots.

The result in Table II shows that the whole–body IK solverdominates over 95% of the online computation time, whichcurrently only runs on a single–thread but can be parallelisedon multi–threaded CPU/GPU. The future work will includeinvestigating parallelised implementation of the IK solveron GPU to bootstrap sampling and interpolation. This willmake the state space exploration more efficient, so that otherstandard algorithms may be able to find valid solutions withinthe same time window.

REFERENCES

[1] K. Rawlik, M. Toussaint, and S. Vijayakumar, “On stochastic optimalcontrol and reinforcement learning by approximate inference,” RSS,2012.

(a) Reach and grasp target on table without facing target.

(b) Reach and grasp target on top of box.

(c) Reach and grasp target on top of shelf.

Fig. 6: Collision–free whole–body motion execution on the NASA Valkyrie humanoid robot. In each scenario, the first figurehighlights the motion plan, followed by execution snapshots.

[2] N. Ratliff, M. Zucker, J. A. Bagnell, and S. Srinivasa, “CHOMP:Gradient optimization techniques for efficient motion planning,” inICRA, IEEE, 2009.

[3] Y. Yang, V. Ivan, and S. Vijayakumar, “Real-time motion adaptationusing relative distance space representation,” in ICAR, IEEE, 2015.

[4] Y. Yang, V. Ivan, Z. Li, M. Fallon, and S. Vijayakumar, “iDRM:Humanoid Motion Planning with Real-Time End-Pose Selection inComplex Environments,” in Humanoids (submitted to), 2016.

[5] S. M. Lavalle, “Rapidly-Exploring Random Trees: A New Tool forPath Planning,” tech. rep., 1998.

[6] L. E. Kavraki, P. Svestka, J. C. Latombe, and M. H. Overmars, “Prob-abilistic roadmaps for path planning in high-dimensional configurationspaces,” ICRA, 1996.

[7] J. J. Kuffner and S. M. LaValle, “RRT-connect: An efficient approachto single-query path planning,” in ICRA, IEEE, 2000.

[8] D. Hsu, J. C. Latombe, and R. Motwani, “Path planning in expansiveconfiguration spaces,” in ICRA, IEEE, 1997.

[9] S. Karaman and E. Frazzoli, “Sampling-based algorithms for optimalmotion planning,” IJRR, pp. 846–894.

[10] I. A. Şucan and L. E. Kavraki, “Kinodynamic motion planningby interior-exterior cell exploration,” in Algorithmic Foundation ofRobotics, 2009.

[11] M. Elbanhawi and M. Simic, “Sampling-Based Robot Motion Plan-ning: A Review,” IEEE Access, 2014.

[12] J. Kuffner, K. Nishiwaki, S. Kagami, M. Inaba, and H. Inoue, “Motionplanning for humanoid robots,” in ISRR, 2005.

[13] K. Hauser, T. Bretl, K. Harada, and J.-C. Latombe, “Using mo-tion primitives in probabilistic sample-based planning for humanoidrobots,” in Algorithmic foundation of robotics, 2008.

[14] M. Cognetti, P. Mohammadi, and G. Oriolo, “Whole-body motionplanning for humanoids based on CoM movement primitives,” inHumanoids, IEEE, 2015.

[15] P. E. Gill, W. Murray, and M. A. Saunders, “SNOPT: An SQPAlgorithm for Large-Scale Constrained Optimization,” SIAM, 2005.

[16] V. Ivan, Y. Yang, and M. Camilleri, “EXOTica: a library for easycreation of tools for optimisation and planning,” 2016.

[17] I. A. Şucan, M. Moll, and L. E. Kavraki, “The Open Motion PlanningLibrary,” Robotics Automation Magazine, IEEE, 2012.

[18] G. Sánchez and J.-C. Latombe, “A single-query bi-directional prob-abilistic roadmap planner with lazy collision checking,” in RoboticsResearch, 2003.

[19] A. Hornung, K. M. Wurm, M. Bennewitz, C. Stachniss, and W. Bur-gard, “OctoMap: An Efficient Probabilistic 3D Mapping FrameworkBased on Octrees,” Autonomous Robots, 2013.

edinburgh research explorer · assume the quasi–static balance constraint and joint limits...

Documents