Humanoid Locomotion Planning

Rosen Diankov (rdiankov _a_t_ cs dot cmu dot edu)

 

Introduction

This project plans walking trajectories for a small 20 degree of freedom humanoid robot called Choromet made by General Robotix. For now, only statically stable trajectories are searched for over the space of all possible walking trajectories. Dynamic trajectories require precise measurements of the center of mass of the robot, it’s exact joint state, and its Zero Moment Point. Although these values can be computed in real time since the robot has force-torque sensors at its feet, they are still imprecise.

 

 

Architecture

 

The OpenRAVE Planning Test Bed developed by me and Dmitry Berenson (both part of James Kuffner’s group) is used for all planning and control of the robot. Choromet has a Sh4 CPU that runs a small server waiting for connections. The client for Choromet is embedded as a plugin DLL to OpenRAVE. Once the humanoid planner plugin successfully plans a trajectory, it sends its commands to an abstract controller interface, which is then responsible for moving Choromet along it. At the moment, the design is very open loop since sensing data is not needing from the robot.

 

 

Each leg has 6 degrees of freedom and each arm has 4 degrees of freedom. The inverse kinematics of the legs are unknown (General Robotix kindly provides compiled binaries of the IK for Sh4 CPUs, but not the source code). The real center of mass is also unknown. Although there is a VRML file that looks like it has careful measurements of the center of mass on a version of the robot with and without its armor, the computed center of mass is still ~1cm off from the real center of mass. This can cause the robot to fall over easily if the trajectory isn’t planned well.

 

Waking Outline

 

Planning is performed by searching for feasible configurations of the robot along every step it makes. There are various ways to formulate the configuration space of the robot:

 

Foot Step Planning

 

This is the method currently applied to ASIMO and the CMU LittleDog and has been researched greatly by Joel Chestnutt. The robot’s kinematics are ignored and only a 2D representation of its feet is used for planning. The search is usually computed by A* or Rapidly-exploring Randomized Trees. Each node in the search tree represents the robot’s 2D position of the feet on the floor and their orientation. For every robot configuration, there are a limited set of actions that the robot can do in order to move around. For example it can choose to move its left foot or its right foot (this can be expanded to hands if desired). After the robot has chosen the end effector, a set of possible foot placement locations are then sampled around the current foot location. Each of these new configurations produce a new node in the graph. The key in foot step planning is creating the heuristics and limiting the branching factor at each node.

 

Stance Planning for Humanoids

 

Initially proposed by Kris Hauser, Tim Bretl, and Latombe in Non-Gaited Humanoid Locomotion Planning. Instead of performing a forward search and relying on heuristics to guide the search, we can plan for stances. Here each stance is a set of contact points that the robot needs to make with the environment. The contact points are chosen in such a way that there exists a configuration of the robot that is statically stable. A series of stances are planned for the robot that it can use to travel from an initial location to the goal location without considering the actual feasibility of going from one stance to another. Once all the stances are planned, a Probabilistic Roadmap Planner is executed to find a trajectory of the robot that goes through some of the stances to get to its goal location.

 

Current Approach

 

The robot is divided into 15 degrees of freedom: 12 for both legs and 3 for the translational position of the torso. We apply a forward based search much like Foot Step Planning except that this is in 15 dimensions. The algorithm is divided into two planning stages: a local planner is responsible for taking one step or shifting the center of mass, a global planner is responsible for determining footstep and center of mass goal locations. The global planner can be summarized by:

 

  1. Shift center of mass to foot #1
  2. Left foot #2 a little (~1 cm)
  3. Set goal of foot #2 on top of the surface directly in front of the robot
    1. Constrain so that foot normal is equal to the surface normal at that location

 

Local Planner

 

Before a planner can be used to search for good steps of the robot, we have to develop a sampling algorithm that can produce randomized statically stable configurations of the robot. Sampling the 15 dimensional configuration space directly and rejecting samples that don’t meet the constraints is way too slow to be practical. The reason is that good robot configurations lie on a sub-manifold that is much lower in dimension. Searching in 15 continuous dimensions is hard and the search algorithm can easily get lost sampling infeasible configurations. The constrains we employ to ensure good configuration are:

·         Ground contact – For stepping, only one foot has to maintain the ground contact. For center of mass shifting, both feet have to maintain their ground contact.

·         Collisions – no collisions with the environment or with separate links of the robot.

·         Balance – center of mass has to be within the convex hull of the foot support

 

Throughout literature, many methods related have been proposed to sample well from constrained configuration spaces:

 

Ground Contact Constraints

 

When stepping once, the only specifications are that one foot maintain its ground contact and the other foot is parallel to the surface directly below it. First use up 3 DOF to make sure the ground foot’s orientation is the same. Then translate all the links of the robot to get the position constraint. This will leave the other foot floating, just use 2 DOF (on the ankles) to make it point to the ground. Sometimes constraining both feet is necessary when shifting the center of mass. Since the IK for the feet don’t exist, use gradient descent using the Jacobian [3] on the remaining 3 DOF of the floating leg to get it to the correct position. This procedure is iterative, so the floating foot will have to be reoriented after every iteration since the descent operation will disregard this constraint. A better way of performing center of mass shifts is to just solve for the IK of the feet directly given the torso position. This reduces the search to 3 degrees of freedom or less, so it will be faster.

 

 

Other Constraints

 

For balance, use the Graham Scan Algorithm on the project of the feet to the ground plane to get a convex hull. Score the balance of the robot by how much its center of mass is inside this convex hull. A good metric for this measure is the minimum distance of the projected center of mass to one of the edges of the convex hull. It is hard to find a projection of a self-colliding configuration to their nearest feasible configuration. In this case, just resample a new configuration.

 

Experimental Results

 

Convergence is pretty fast when some constraints are relaxed. The bottom is a video of the robot taking a big step.

 

 

Here is the search graph for the big walk using a randomized version of A*. Each subplot displays 3 of the 15 search dimensions. Each point is a sampled configuration of the robot. Blue points are close to the goal, Red points are farther (the start point is the most red).

 

Walking

 

Here is a movie of one walking trajectory computed in simulation:

 

Real Hardware

 

There was a lot of problems making the real robot balance because the real center of mass is unknown (and hard to calculate since the joints shift a lot). Due to the searching algorithm producing optimal (shortest) plans, the feet are very close to the ground. Sometimes the real robot will trip on them. This can be compensated by adding a term that tells the robot to stay away from the ground, but then convergence will be slower. Another problem is that the robot has to move very slow when executing the trajectory. Moving fast introduces unwanted dynamics and vibrations since the motors are servos.

 

Here is a movie of the real robot taking one step (sped up by 2x).

 

References

 

[1] – J. Cortes, T. Simeon, J.P. Laumond. A Random Loop Generator for Planning the Motions of Closed Kinematic Chains using PRM Methods. Intl Conf on Robotics and Automation, 2002.

[2] – K. Hauser, T. Bretl, J. Latombe. Non-Gaited Humanoid Locomotion Planning. Intl Conf on Humanoids Robots, 2005.

[3] - Y. Nakamura, H. Hanafusa. Inverse Kinematics solutions with Singularity Robustness for Robot Manipulator Control. Journal of Dynamic Systems, Measurement, and Control. 108 (1986), pp. 163-171.

 

Side note: If anyone is curious about OpenRAVE and how to use it in their planning projects just send me an email.