Humanoid robots dynamically navigate an environment by interacting with it via contact wrenches exerted at intermittent contact poses. Therefore, it is important to consider dynamics when planning a contact sequence. Traditional contact planning approaches assume a quasi-static balance criterion to reduce the computational challenges of selecting a contact sequence over a rough terrain. This however limits the applicability of the approach when dynamic motions are required, such as when walking down a steep slope or crossing a wide gap. Recent methods overcome this limitation with the help of efficient mixed integer convex programming solvers capable of synthesizing dynamic contact sequences. Nevertheless, its exponential-time complexity limits its applicability to short time horizon contact sequences within small environments. In this paper, we go beyond current approaches by learning a prediction of the dynamic evolution of the robot centroidal momenta, which can then be used for quickly generating dynamically robust contact sequences for robots with arms and legs using a search-based contact planner. We demonstrate the efficiency and quality of the results of the proposed approach in a set of dynamically challenging scenarios.
@cite_32 proposes a kinodynamic sampling-based contact planner to plan kinodynamically feasible contact sequences. They use a simplified robot model to dynamically plan smooth center of mass (CoM) trajectories based on convex optimization and then search for kinematically feasible contact poses around it. It shows a unified planning framework to consider dynamics and kinematics constraints, but it suffers from long planning time. @cite_13 proposes an efficient dynamic feasibility check by conservatively reformulating the problem as a linear program. While the check guarantees to reject dynamically infeasible motions, they do not address dynamical robustness in the stability check. @cite_2 learns quadratic dynamics objective of humanoid walking motion, and apply this learned model to select steps in a search-based footstep planner. However, their dynamics model assumes flat contact, and does not consider palm contacts, which limits the applicability of the approach.
@cite_13 @cite_32 @cite_2
2789286310 2597700484 2029068927
Most animal and human locomotion behaviors for solving complex tasks involve dynamic motions and rich contact interaction. In fact, complex maneuvers need to consider dynamic movement and contact events at the same time. We present a hierarchical trajectory optimization approach for planning dynamic movements with unscheduled contact sequences. We compute whole-body motions that achieve goals that cannot be reached in a kinematic fashion. First, we find a feasible CoM motion according to the centroidal dynamics of the robot. Then, we refine the solution by applying the robot's full-dynamics model, where the feasible CoM trajectory is used as a warm-start point. To accomplish the unscheduled contact behavior, we use complementarity constraints to describe the contact model, i.e. environment geometry and non-sliding active contacts. Both optimization phases are posed as Mathematical Program with Complementarity Constraints (MPCC). Experimental trials demonstrate the performance of our planning approach in a set of challenging tasks. We tackle the transition feasibility problem, that is the issue of determining whether there exists a feasible motion connecting two configurations of a legged robot. To achieve this we introduce CROC, a novel method for computing centroidal dynamics trajectories in multi-contact planning contexts. Our approach is based on a conservative and convex reformulation of the problem, where we represent the center of mass trajectory as a Bezier curve comprising a single free control point as a variable. Under this formulation, the transition problem is solved efficiently with a Linear Program (LP)of low dimension. We use this LP as a feasibility criterion, incorporated in a sampling-based contact planner, to discard efficiently unfeasible contact plans. We are thus able to produce robust contact sequences, likely to define feasible motion synthesis problems. We illustrate this application on various multi-contact scenarios featuring HRP2 and HyQ. We also show that we can use CROC to compute valuable initial guesses, used to warm-start non-linear solvers for motion generation methods. This method could also be used for the 0 and 1-Step capturability problem. The source code of CROC is available under an open source BSD-2 License. This paper presents a generic and efficient approach to generate dynamically consistent motions for under-actuated systems like humanoid or quadruped robots. The main contribution is a walking pattern generator, able to compute a stable trajectory of the center of mass of the robot along with the angular momentum, for any given configuration of contacts (e.g. on uneven, sloppy or slippery terrain, or with closed-gripper). Unlike existing methods, our solver is fast enough to be applied as a model-predictive controller. We then integrate this pattern generator in a complete framework: an acyclic contact planner is first used to automatically compute the contact sequence from a 3D model of the environment and a desired final posture; a stable walking pattern is then computed by the proposed solver; a dynamically-stable whole-body trajectory is finally obtained using a second-order hierarchical inverse kinematics. The implementation of the whole pipeline is fast enough to plan a step while the previous one is executed. The interest of the method is demonstrated by real experiments on the HRP-2 robot, by performing long-step walking and climbing a staircase with handrail support. This paper introduces an optimization-based framework for robust multilegged walking motion planning. Previous approaches use fixed gait sequences, and rely on Zero Moment Point (ZMP) to guarantee dynamic stability. While this combination works well on flat ground, it does not generalize to uneven terrain requiring aggressive gait or gait transition. To overcome such difficulties, in this paper, we present an optimization framework, that can plan both the contact location and gait sequence simultaneously in a mixed-integer convex optimization program. Moreover, we rely on the Contact Wrench Cone (CWC) stability criterion, which generalizes the ZMP criterion to uneven terrain with friction cone constraints, and we plan the walking motion together with the angular momentum through a convex optimization program. Our approach is successfully tested on a LittleDog quadruped over simulated scenarios. We show that on the flat ground, our planner generates a periodic gait, same as Central Pattern Generator + ZMP planner; while on uneven terrain, our planner can successfully generate a motion containing different gaits, with a center-of-mass motion that respects the friction cone constraints, which are violated by ZMP planners. This improvement clearly demonstrates the advantage of our approach over traditional planning strategies. In this paper, we present a convex optimization problem to generate Center of Mass (CoM) and momentum trajectories of a walking robot, such that the motion robustly satisfies the friction cone constraints on uneven terrain. We adopt the Contact Wrench Cone (CWC) criterion to measure a robot's dynamical stability, which generalizes the venerable Zero Moment Point (ZMP) criterion. Unlike the ZMP criterion, which is ideal for walking on flat ground with unbounded tangential friction forces, the CWC criterion incorporates non-coplanar contacts with friction cone constraints. We measure the robustness of the motion using the margin in the Contact Wrench Cone at each time instance, which quantifies the capability of the robot to instantaneously resist external force torque disturbance, without causing the foot to tip over or slide. For pre-specified footstep location and time, we formulate a convex optimization problem to search for robot linear and angular momenta that satisfy the CWC criterion. We aim to maximize the CWC margin to improve the robustness of the motion, and minimize the centroidal angular momentum (angular momentum about CoM) to make the motion natural. Instead of directly minimizing the non-convex centroidal angular momentum, we resort to minimizing a convex upper bound. We show that our CWC planner can generate motion similar to the result of the ZMP planner on flat ground with sufficient friction. Moreover, on an uneven terrain course with friction cone constraints, our CWC planner can still find feasible motion, while the outcome of the ZMP planner violates the friction limit. Abstract : To plan dynamic, whole-body motions for robots one conventionally faces the choice between a complex, fullbody dynamic model containing every link and actuator of the robot, or a highly simplified model of the robot as a point mass. In this paper we explore a powerful middle ground between these extremes. We present an approach to generate whole-body motions using a simple dynamics model, which enforces that the linear and angular momentum of the robot be consistent with the external wrenches on the robot, and a full-body kinematics model that enforces rich geometric constraints, such as end-effector positioning or collision avoidance. We obtain a trajectory for the robot and profiles of contact wrenches by solving a nonlinear optimization problem (NLP). We further demonstrate that we can plan without pre-specifying the contact sequence by exploiting the complementarity conditions between contact forces and contact distance. We demonstrate that this algorithm is capable of generating highly-dynamic motion plans with examples of a humanoid robot negotiating obstacle course elements and gait optimization for a quadrupedal robot. This paper presents the contact-consistent elastic strips (CES) framework, a motion planning approach capable of producing complex multi-contact whole-body humanoid behaviors in dynamic environments. Planning multi-contact motions for humanoid robots is known to be non-trivial since it involves aspects of autonomous balancing, obstacle avoidance, ensuring global connectivity of the workspace and concurrent consideration of the kinematic and dynamic constraints. Previous works at motion planning for humanoid systems tend to focus on joint space planning and deal with obstacle avoidance and contact-point searching as the separated problems. CES framework, however, simultaneously considers all these requirements and constraints in task space while planning a valid sequence of contact-points and corresponding motions. This resulted in considerable improvements to efficiency and significantly reduced planning time. With the use of CES framework, complex multi-contact locomotion behaviors and real-time adjustments to the robot motions in 3D unstructured environments is possible. Several simulations are demonstrated to evaluate and verify the performance of CES framework. The objective of this paper is to present a full-stack, real-time kinodynamic planning framework and demonstrate it on a quadrotor for collision avoidance. Specifically, the proposed framework utilizes an offlineonline computation paradigm, neighborhood classification through machine learning, sampling-based motion planning with an optimal control distance metric, and trajectory smoothing to achieve real-time planning for aerial vehicles. The approach is demonstrated on a quadrotor navigating obstacles in an indoor space and stands as, arguably, one of the first demonstrations of full-online kinodynamic motion planning; exhibiting execution times under 1 3 of a second. For the quadrotor, a simplified dynamics model is used during the planning phase to accelerate online computation. A trajectory smoothing phase, which leverages the differentially flat nature of quadrotor dynamics, is then implemented to guarantee a dynamically feasible trajectory. We present a multi-contact walking pattern generator based on preview-control of the 3D acceleration of the center of mass (COM). A key point in the design of our algorithm is the calculation of contact-stability constraints. Thanks to a mathematical observation on the algebraic nature of the frictional wrench cone, we show that the 3D volume of feasible COM accelerations is always an upward-pointing cone. We reduce its computation to a convex hull of (dual) 2D points, for which optimal C(nlog n) algorithms are readily available. This reformulation brings a significant speedup compared to previous methods, which allows us to compute time-varying contact-stability criteria fast enough for the control loop. Next, we propose a conservative trajectory-wide contact-stability criterion, which can be derived from COM-acceleration volumes at marginal cost and directly applied in a model-predictive controller. We finally implement this pipeline and exemplify it with the HRP-4 humanoid model in multi-contact dynamically walking scenarios. We present a novel method for synthesizing collision-free, dynamic locomotion behaviors for legged robots, including jumping, going down a very steep slope, or recovering from a push using the arms of the robot. The approach is automatic and generic: non-gaited motions, comprising arbitrary contact postures can be generated along any environment. At the core of our framework is a new steering method that generates trajectories connecting two states of the robot. These trajectories account for the state-dependent, centroidal dynamic constraints inherent to legged robots. The method, of low dimension, formulated as a Linear Program, is really efficient to compute, and can find an application in various problems related to legged locomotion. By incorporating this steering method into an existing sampling-based contact planner, we propose the first kinodynamic contact planner for legged robots. Transferring solutions found by trajectory optimization to robotic hardware remains a challenging task. When the optimization fully exploits the provided model to perform dynamic tasks, the presence of unmodeled dynamics renders the motion infeasible on the real system. Model errors cannot be only a result of model simplifications, but also naturally arise when deploying the robot in unstructured and nondeterministic environments. Predominantly, compliant contacts and actuator dynamics lead to bandwidth limitations. While classical control methods provide tools to synthesize controllers that are robust to a class of model errors, such a notion is missing in modern trajectory optimization, which is solved in the time domain. We propose frequency-shaped cost functions to achieve robust solutions in the context of optimal control for legged robots. Through simulation and hardware experiments we show that motion plans can be made compatible with bandwidth limits set by actuators and contact dynamics. The smoothness of the model predictive solutions can be continuously tuned without compromising the feasibility of the problem. Experiments with the quadrupedal robot ANYmal, which is driven by highly compliant series elastic actuators, showed significantly improved tracking performance of the planned motion, torque, and force trajectories and enabled the machine to walk robustly on terrain with unmodeled compliance. Planning motions to grasp an object in cluttered and uncertain environments is a challenging task, particularly when a collision-free trajectory does not exist and objects obstructing the way are required to be carefully grasped and moved out. This letter takes a different approach and proposes to address this problem by using a randomized physics-based motion planner that permits robot-object and object-object interactions. The main idea is to avoid an explicit high-level reasoning of the task by providing the motion planner with a physics engine to evaluate possible complex multibody dynamical interactions. The approach is able to solve the problem in complex scenarios, also considering uncertainty in the objects' pose and in the contact dynamics. The work enhances the state validity checker, the control sampler, and the tree exploration strategy of a kinodynamic motion planner called KPIECE. The enhanced algorithm, called p-KPIECE, has been validated in simulation and with real experiments. The results have been compared with an ontological physics-based motion planner and with task and motion planning approaches, resulting in a significant improvement in terms of planning time, success rate, and quality of the solution path. Humanoid robots have abilities of stepping over or onto obstacles, which is different from wheeled robots. However it may be difficult to apply the ordinary motion planning methods such as Rapidly-exploring Random Trees (RRT) to humanoid robots directly. Because these kinds of methods only consider to circumvent obstacles and ignore the constraint of balance. Aiming at dealing with these problems in one frame, a novel approach based on hierarchical RRT is used to plan the footstep for humanoid robots. It is designed according to three basic constraints: a transition model based gait generator, an inverted pendulum based balance controller and a collision detection based path planner. First, a set of layered transition model is utilized to revise the footstep according to the terrain condition, which is able to take full use of the motion ability as well as improve the efficiency. Then, a hierarchical strategy is exploited to select the feasible foot location to be added in the random tree based on the results of collision checking and balance control. Finally, a dynamic RRT method is introduced in our work to revise paths in changing environments. Different experiments are given to verify the feasibility and performance of the proposed approach in complicated environments with both dynamic and static obstacles. Kinematic planning for robotic arms has been shown to be capable of planning for robust manipulation of static objects. However, this approach falls short when manipulating moving objects such as picking up a jar off of a conveyor belt at a bottling plant. The challenge in carefully picking up moving objects is that these actions require motions that do not involve large decelerations, to avoid jerking the object, as well as figuring out the proper time in which the object can be picked up. We present a search-based kinodynamic motion planning algorithm that generates a timeparameterized trajectory for both the arm and end-effector, capable of carefully picking up the object at the earliest feasible point in its trajectory. To combat the high-dimensionality of the time-parameterized kinodynamic planning problem, our approach employs informative heuristics and adaptive dynamic motion primitives. To validate our approach, we used a 7DOF manipulator on Willow Garage’s PR2 robot to pickup objects off of a conveyor belt. We also provide a detailed set of results that demonstrate the planner’s ability to generate consistent, low cost trajectories for manipulation. We consider motion planning problems for a vehicle with kinodynamic constraints, where there is partial knowledge about the environment and replanning is required. We present a new tree-based planner that explicitly deals with kinodynamic constraints and addresses the safety issues when planning under finite computation times, meaning that the vehicle avoids collisions in its evolving configuration space. In order to achieve good performance we incrementally update a tree data-structure by retaining information from previous steps and we bias the search of the planner with a greedy, yet probabilistically complete state space exploration strategy. Moreover, the number of collision checks required to guarantee safety is kept to a minimum. We compare our technique with alternative approaches as a standalone planner and show that it achieves favorable performance when planning with dynamics. We have applied the planner to solve a challenging replanning problem involving the mapping of an unknown workspace with a nonholonomic platform This paper explores the problem of autonomous, in-hand regrasping-the problem of moving from an initial grasp on an object to a desired grasp using the dexterity of a robot's fingers. We propose a planner for this problem which alternates between finger gaiting, and in-grasp manipulation. Finger gaiting enables the robot to move a single finger to a new contact location on the object, while the remaining fingers stably hold the object. In-grasp manipulation moves the object to a new pose relative to the robot's palm, while maintaining the contact locations between the hand and object. Given the object's geometry (as a mesh), the hand's kinematic structure, and the initial and desired grasps, we plan a sequence of finger gaits and object reposing actions to reach the desired grasp without dropping the object. We propose an optimization based approach and report in-hand regrasping plans for 5 objects over 5 in-hand regrasp goals each. The plans generated by our planner are collision free and guarantee kinematic feasibility. Kinodynamic RRT planners are considered to be general tools for effectively finding feasible trajectories for high-dimensional dynamical systems. However, they struggle when holonomic constraints are present in the system, such as those arising in parallel manipulators, in robots that cooperate to fulfill a given task, or in situations involving contacts with the environment. In such cases, the state space becomes an implicitly-defined manifold, which makes the diffusion heuristic inefficient and leads to inaccurate dynamical simulations. To address these issues, this paper presents an extension of the kinodynamic RRT planner that constructs an atlas of the state-space manifold incrementally, and uses this atlas both to generate random states and to dynamically steer the system towards such states. To the best of our knowledge, this is the first randomized kinodynamic planner that explicitly takes holonomic constraints into account. We validate the approach in significantly-complex systems. We propose a humanoid robot navigation planning framework that reuses previous experience to decrease planning time. The framework is intended for navigating complex unstructured environments using both palm and foot contacts. In a complex environment, discrete-search-based contact space planners trade-off between high branching factor and action flexibility. Although approaches such as weighted A∗, ARA∗ and ANA∗ could speed up the search by compromising on optimality, they can be very slow when the heuristic is inaccurate. In the proposed framework, an experience-retrieval module is added in parallel to ANA∗. This module collects previously-generated motion plans and clusters them based on contact pose similarity to form a motion plan library. To retrieve an appropriate plan from the library for a given environment, the framework uses a distance between the contact poses in the plan and environment surfaces. Candidate plans are then modified with local trajectory optimization until a plan fitting the query environment is found. Our experiments show that the proposed framework outperforms planning-from-scratch in success rate in unstructured environments by at least 28 and can navigate difficult environments such as rubble and narrow corridors. Contact constraints, such as those between a foot and the ground or a hand and an object, are inherent in many robotic tasks. These constraints define a manifold of feasible states; while well understood mathematically, they pose numerical challenges to many algorithms for planning and controlling whole-body dynamic motions. In this paper, we present an approach to the synthesis and stabilization of complex trajectories for both fully-actuated and underactuated robots subject to contact constraints. We introduce a trajectory optimization algorithm (DIRCON) that extends the direct collocation method, naturally incorporating manifold constraints to produce a nominal trajectory with third-order integration accuracy—a critical feature for achieving reliable tracking control. We adapt the classical time-varying linear quadratic regulator to produce a local cost-to-go in the manifold tangent plane. Finally, we descend the cost-to-go using a quadratic program that incorporates unilateral friction and torque constraints. This approach is demonstrated on three complex walking and climbing locomotion examples in simulation. We present a contact planner for complex legged locomotion tasks: standing up, climbing stairs using a handrail, crossing rubble, and getting out of a car. The need for such a planner was shown at the DARPA Robotics Challenge, where such behaviors could not be demonstrated (except for egress). Current planners suffer from their prohibitive algorithmic complexity because they deploy a tree of robot configurations projected in contact with the environment. We tackle this issue by introducing a reduction property: the reachability condition. This condition defines a geometric approximation of the contact manifold, which is of low dimension, presents a Cartesian topology, and can be efficiently sampled and explored. The hard contact planning problem can then be decomposed into two subproblems: first, we plan a path for the root without considering the whole-body configuration, using a sampling-based algorithm; then, we generate a discrete sequence of whole-body configurations in static equilibrium along this path, using a deterministic contact-selection algorithm. The reduction breaks the algorithm complexity encountered in previous works, resulting in the first interactive implementation of a contact planner (open source). While no contact planner has yet been proposed with theoretical completeness, we empirically show the interest of our framework: in a few seconds, with high success rates, we generate complex contact plans for various scenarios and two robots: HRP-2 and HyQ. These plans are validated in dynamic simulations or on the real HRP-2 robot.