This project aims simulating a humanoid robot’s walking and balancing using Whole Body Control (WBC) and Divergent Component of Motion (DCM) planner using URDF models of Boston Dynamics’ ATLAS in PyBullet enviroment. This project was implemented from PyPnC as a part of The Unversity of Texas as Austin’s Design and Control of Human Centered Robotics (ASE389) course.

Humanoid Modeling under Multi-Contact Constraints

Challenges in humanoid robot involve high dimensionality, underactuation, and operation under geometrical and multi-contact constraints. A humanoid robot’s behavior is determined by not only the dispalcements of its articulated joints, but also interactions of its bodies in contact.

Figure 1: Kinematic representation of a humanoid robot The free moving base is represented as a virtual spherical joint in series with three prismatic virtual joints. Reaction forces appear at the contact points due to gravity forces pushing the body against the ground. Contact constraints are expressed as rigid constraints with zero velocities and accelerations at the supporting bodies [1].

The equation of motion (EOM) which is subjected to acutation torques and external forces can be derived using Euler-Lagrange equation

where A is the (n + 6) × (n + 6) inertia matrix, b and g are (n + 6) × 1 vectors of Coriolis/centrifugal and gravity forces. For more detail, refer to Compliant Control of Whole-Body Multi-Contact Behaviors in Humanoid Robots

Joint Space Control, Operational Space Control, and Whole Body Control

Figure 2: A 3-link planar robot arm

EOM above can be developed into different control schemes, and they can be shown with a simple 3-link planar robot arm which is more intuitive than a complex humanoid robot to explore features of each control scheme.

Figure 3: Joint Space Control with PD Controller

Firstly, joint space control takes desired joint configuration as an input and outputs joint torques with feedback control.

Figure 4: Operational Space Control with PD Controller

Secondly, operational space control takes control the end-effector’s motion in the task space by controlling the forces and torques in the control space. Operational space control is more high-level and task-specific control as it enables the specification of desired end-effector behaviors in the task space, rather than specifying joint angles or velocities in the control space.

Figure 5: Priortized Whole Body Control (Task and Posture Control)

Finally, prioritzed whole body control can perform two tasks which conflict with each other. Figure 5 shows the desired joint configuration as [0,0,0] (task: x2) which configures the robot strech in a straight line while attempting to achieve the desired end-effector orientation as pi/4 (task: x1) which is not feasible with the desired joint configuration at [0,0,0]. Priortized whole body control law can set a higher priority of task x1 over task x2. In this case, the robot achieves the desired end-effector orientation while trying to minimize the error in the desired joint configuration. This control law ensures accomplishment of critical tasks over sub-tasks, and it enables whole-body compliant behaviors and exploit the redundancy of humanoids under geometric and contact constraints.

Whole Body Control: Stand and Balance

WBC for humanoid balancing can be formulated with optimization problem which minimizes the joint acceleration and reaction forces. It can be represented as

The optimization is subjected to the cost function, equality constraints, and inequality constraints, and boundaries of joint accleration and command torques. The optimization problem is solved with quadratic programming (QP) solver.

The cost function is a quadratic function of joint acceleration and reaction forces. The equality constraints are the EOM and the kinematic constraints which allow the robot to stand on the ground by the law of physics. The inequality constraints include the wrench term while the inequality vector is determined by the gravity and Coriolis term. The wrench term ensures that the direction of the normal force under gravity and Coriolis force prevent the foot from slipping and flipping. While Coulomb friction law defines the relationship between the frictional and normal force of two surfaces in contact, inequality constraints enforce a minimum normal force during the optimization. The inequality constraints also prevents saturation of the joint acceleration and command torques.

Figure 6: Humanoid balancing with WBC

DCM Planner: Trajectory Generation

While DCM is a quantity in Cartesian space that is related to the COM position and velocity, DCM Planner generates walking pattern using virtual repellent point (VRP) which is a point in Cartesian space that encodes both direction and magnitude of the external forces acting on the robot.

Figure 7: DCM and VRP: COM tends to covnerge to DCM while VRP repel DCM

Figure 8: Forward Walking Trajectory (Top) and Backward Walking Trajectory (Bottom)

Foward, Backward, and Sideway Walking

Click to watch the video