Biped

Overview

This tutorial demonstrates the dynamic features in DART useful for developing controllers for bipedal or wheel-based robots. The tutorial consists of seven Lessons covering the following topics:

• Joint limits and self-collision.
• Actuators types and management.
• APIs for Jacobian matrices and other kinematic quantities.
• APIs for dynamic quantities.
• Skeleton editing.

Please reference the source code in tutorialBiped.cpp and tutorialBiped-Finished.cpp.

Lesson 1: Joint limits and self-collision

Let’s start by locating the main function in tutorialBiped.cpp. We first create a floor and call loadBiped to load a bipedal figure described in SKEL format, which is an XML format representing a robot model. A SKEL file describes a World with one or more Skeletons in it. Here we load in a World from biped.skel and assign the bipedal figure to a Skeleton pointer called biped.

SkeletonPtr loadBiped()
{
...
    WorldPtr world = SkelParser::readWorld(DART_DATA_PATH"skel/biped.skel");
    SkeletonPtr biped = world->getSkeleton("biped");
...
}

Running the skeleton code (hit the spacebar) without any modification, you should see a human-like character collapse on the ground and fold in on itself. Before we attempt to control the biped, let’s first make the biped a bit more realistic by enforcing more human-like joint limits.

DART allows the user to set upper and lower bounds on each degree of freedom in the SKEL file or using provided APIs. For example, you should see the description of the right knee joint in biped.skel:

<joint type="revolute" name="j_shin_right">
...
    <axis>
        <xyz>0.0 0.0 1.0</xyz>
        <limit>
            <lower>-3.14</lower>
            <upper>0.0</upper>
        </limit>
    </axis>
...
</joint>

The <upper> and <lower> tags make sure that the knee can only flex but not extend. Alternatively, you can directly specify the joint limits in the code using setPositionUpperLimit and setPositionLowerLimit.

In either case, the joint limits on the biped will not be activated until you call setPositionLimited:

SkeletonPtr loadBiped()
{
...
    for(size_t i = 0; i < biped->getNumJoints(); ++i)
        biped->getJoint(i)->setPositionLimited(true);
...
}

Once the joint limits are set, the next task is to enforce self-collision. By default, DART does not check self-collision within a skeleton. You can enable self-collision checking on the biped by

SkeletonPtr loadBiped()
{
...
    biped->enableSelfCollision();
...
}

This function will enable self-collision on every non-adjacent pair of body nodes. If you wish to also enable self-collision on adjacent body nodes, set the optional parameter to true:

biped->enableSelfCollision(true);

Running the program again, you should see that the character is still floppy like a ragdoll, but now the joints do not bend backward and the body nodes do not penetrate each other anymore.

Lesson 2: Proportional-derivative control

To actively control its own motion, the biped must exert internal forces using actuators. In this Lesson, we will design one of the simplest controllers to produce internal forces that make the biped hold a target pose. The proportional-derivative (PD) control computes control force by Τ = -k_p (θ - θ_target) - k_d θ̇, where θ and θ̇ are the current position and velocity of a degree of freedom, θ_target is the target position set by the controller, and k_p and k_d are the stiffness and damping coefficients. The detailed description of a PD controller can be found here.

The first task is to set the biped to a particular configuration. You can use setPosition to set each degree of freedom individually:

void setInitialPose(SkeletonPtr biped)
{
...
    biped->setPosition(biped->getDof("j_thigh_left_z")->getIndexInSkeleton(), 0.15);
...
}

Here the degree of freedom named “j_thigh_left_z” is set to 0.15 radian. Note that each degree of freedom in a skeleton has a numerical index which can be accessed by getIndexInSkeleton. You can also set the entire configuration using a vector that holds the positions of all the degreed of freedoms using setPositions.

We continue to set more degrees of freedoms in the lower body to create a roughly stable standing pose.

biped->setPosition(biped->getDof("j_thigh_left_z")->getIndexInSkeleton(), 0.15);
biped->setPosition(biped->getDof("j_thigh_right_z")->getIndexInSkeleton(), 0.15);
biped->setPosition(biped->getDof("j_shin_left")->getIndexInSkeleton(), -0.4);
biped->setPosition(biped->getDof("j_shin_right")->getIndexInSkeleton(), -0.4);
biped->setPosition(biped->getDof("j_heel_left_1")->getIndexInSkeleton(), 0.25);
biped->setPosition(biped->getDof("j_heel_right_1")->getIndexInSkeleton(), 0.25);

Now the biped will start in this configuration, but will not maintain this configuration as soon as the simulation starts. We need a controller to make this happen. Let’s take a look at the constructor of our Controller in the skeleton code:

Controller(const SkeletonPtr& biped)
{
...
    for(size_t i = 0; i < 6; ++i)
    {
        mKp(i, i) = 0.0;
        mKd(i, i) = 0.0;
    }

    for(size_t i = 6; i < mBiped->getNumDofs(); ++i)
    {
        mKp(i, i) = 1000;
        mKd(i, i) = 50;
    }

    setTargetPositions(mBiped->getPositions());
}

Here we arbitrarily define the stiffness and damping coefficients to 1000 and 50, except for the first six degrees of freedom. Because the global translation and rotation of the biped are not actuated, the first six degrees of freedom at the root do not exert any internal force. Therefore, we set the stiffness and damping coefficients to zero. At the end of the constructor, we set the target position of the PD controller to the current configuration of the biped.

With these settings, we can compute the forces generated by the PD controller and add them to the internal forces of biped using setForces:

void addPDForces()
{
    Eigen::VectorXd q = mBiped->getPositions();
    Eigen::VectorXd dq = mBiped->getVelocities();

    Eigen::VectorXd p = -mKp * (q - mTargetPositions);
    Eigen::VectorXd d = -mKd * dq;

    mForces += p + d;
    mBiped->setForces(mForces);
}

Note that the PD control force is added to the current internal force stored in mForces instead of overriding it.

Now try to run the program and see what happens. The skeleton disappears almost immediately as soon as you hit the space bar! This is because our stiffness and damping coefficients are set way too high. As soon as the biped deviates from the target position, huge internal forces are generated to cause the numerical simulation to blow up.

So let’s lower those coefficients a bit. It turns out that each of the degrees of freedom needs to be individually tuned depending on many factors, such as the inertial properties of the body nodes, the type and properties of joints, and the current configuration of the system. Figuring out an appropriate set of coefficients can be a tedious process difficult to generalize across new tasks or different skeletons. In the next Lesson, we will introduce a much more efficient way to stabilize the PD controllers without endless tuning and trial-and-errors.

Lesson 3: Stable PD control

SPD is a variation of PD control proposed by Jie Tan. The basic idea of SPD is to compute control force using the predicted state at the next time step, instead of the current state. This Lesson will only demonstrate the implementation of SPD using DART without going into details of SPD derivation.

The implementation of SPD involves accessing the current dynamic quantities in Lagrange’s equations of motion. Fortunately, these quantities are readily available via DART API, which makes the full implementation of SPD simple and concise:

void addSPDForces()
{
    Eigen::VectorXd q = mBiped->getPositions();
    Eigen::VectorXd dq = mBiped->getVelocities();

    Eigen::MatrixXd invM = (mBiped->getMassMatrix() + mKd * mBiped->getTimeStep()).inverse();
    Eigen::VectorXd p = -mKp * (q + dq * mBiped->getTimeStep() - mTargetPositions);
    Eigen::VectorXd d = -mKd * dq;
    Eigen::VectorXd qddot = invM * (-mBiped->getCoriolisAndGravityForces() + p + d + mBiped->getConstraintForces());

    mForces += p + d - mKd * qddot * mBiped->getTimeStep();
    mBiped->setForces(mForces);
}

You can get mass matrix, Coriolis force, gravitational force, and constraint force projected onto generalized coordinates using function calls getMassMatrix, getCoriolisForces, getGravityForces, and getConstraintForces, respectively. Constraint forces include forces due to contacts, joint limits, and other joint constraints set by the user (e.g. the weld joint constraint in the multi-pendulum tutorial).

With SPD, a wide range of stiffness and damping coefficients will all result in stable motion. In fact, you can just leave them to our original values: 1000 and 50. By holding the target pose, now the biped can stand on the ground in balance indefinitely. However, if you apply an external push force on the biped (hit ‘,’ or ‘.’ key to apply a backward or forward push), the biped loses its balance quickly. We will demonstrate a more robust feedback controller in the next Lesson.

Lesson 4: Ankle strategy

Ankle (or hip) strategy is an effective way to maintain standing balance. The idea is to adjust the target position of ankles according to the deviation between the center of mass and the center of pressure projected on the ground. A simple linear feedback rule is used to update the target ankle position: θ_a = -k_p (x - p) - k_d (ẋ - ṗ), where x and p indicate the center of mass and center of pressure in the anterior-posterior axis. k_p and k_d are the feedback gains defined by the user.

To implement ankle strategy, let’s first compute the deviation between the center of mass and an approximated center of pressure in the anterior-posterior axis:

void addAnkleStrategyForces()
{
    Eigen::Vector3d COM = mBiped->getCOM();
    Eigen::Vector3d offset(0.05, 0, 0);
    Eigen::Vector3d COP = mBiped->getBodyNode("h_heel_left")->getTransform() * offset;
    double diff = COM[0] - COP[0];
...
}

DART provides various APIs to access useful kinematic information. For example, getCOM returns the center of mass of the skeleton and getTransform returns transformation of the body node with respect to any coordinate frame specified by the parameter (world coordinate frame as default). DART APIs also come in handy when computing the derivative term, -k_d (ẋ - ṗ):

void addAnkleStrategyForces()
{
...
    Eigen::Vector3d dCOM = mBiped->getCOMLinearVelocity();
    Eigen::Vector3d dCOP =  mBiped->getBodyNode("h_heel_left")->getLinearVelocity(offset);
    double dDiff = dCOM[0] - dCOP[0];
...
}

The linear/angular velocity/acceleration of any point in any coordinate frame can be easily accessed in DART. The full list of the APIs for accessing various velocities/accelerations can be found in the API Documentation. The following table summarizes the essential APIs.

The remaining of the ankle strategy implementation is just the matter of parameters tuning. We found that using different feedback rules for falling forward and backward result in more stable controller.

Lesson 5: Skeleton editing

DART provides various functions to copy, delete, split, and merge parts of skeletons to alleviate the pain of building simulation models from scratch. In this Lesson, we will load a skateboard model from a SKEL file and merge our biped with the skateboard to create a wheel-based robot.

We first load a skateboard from skateboard.skel:

void modifyBipedWithSkateboard(SkeletonPtr biped)
{
    WorldPtr world = SkelParser::readWorld(DART_DATA_PATH"skel/skateboard.skel");
    SkeletonPtr skateboard = world->getSkeleton(0);
...
}

Our goal is to make the skateboard Skeleton a subtree of the biped Skeleton connected to the left heel BodyNode via a newly created Euler joint. To do so, you need to first create an instance of EulerJoint::Properties for this new joint.

void modifyBipedWithSkateboard(SkeletonPtr biped)
{
...
    EulerJoint::Properties properties = EulerJoint::Properties();
    properties.mT_ChildBodyToJoint.translation() = Eigen::Vector3d(0, 0.1, 0);
...
}

Here we increase the vertical distance between the child BodyNode and the joint by 0.1m to give some space between the skateboard and the left foot. Now you can merge the skateboard and the biped using this new Euler joint by

void modifyBipedWithSkateboard(SkeletonPtr biped)
{
...
    skateboard->getRootBodyNode()->moveTo<EulerJoint>(biped->getBodyNode("h_heel_left"), properties);
}

There are many other functions you can use to edit skeletons. Here is a table of some relevant functions for quick references.

Lesson 6: Actuator types

DART provides five types of actuator. Each joint can select its own actuator type.

In this Lesson, we will switch the actuator type of the wheels from the default FORCE type to VELOCITY type.

void setVelocityAccuators(SkeletonPtr biped)
{
    Joint* wheel1 = biped->getJoint("joint_front_left");
    wheel1->setActuatorType(Joint::VELOCITY);
...
}

Once all four wheels are set to VELOCITY actuator type, you can command them by directly setting the desired velocity:

void setWheelCommands()
{
...
    int index1 = mBiped->getDof("joint_front_left_2")->getIndexInSkeleton();
    mBiped->setCommand(index1, mSpeed);
...
}

Note that setCommand only exerts commanding force in the current time step. If you wish the wheel to continue spinning at a particular speed, setCommand needs to be called at every time step.

We also set the stiffness and damping coefficients for the wheels to zero.

void setWheelCommands()
{
    int wheelFirstIndex = mBiped->getDof("joint_front_left_1")->getIndexInSkeleton();
    for (size_t i = wheelFirstIndex; i < mBiped->getNumDofs(); ++i)
    {
        mKp(i, i) = 0.0;
        mKd(i, i) = 0.0;
    }
...
}

This is because we do not want the velocity-based actuators to incorrectly affect the computation of SPD. If we use simple PD control scheme, the values of these spring and damping coefficients do not affect the dynamics of the system.

Let’s simulate what we’ve got so far. The biped now is connecting to the skateboard through a Euler joint. Once the simulation starts, you can use ‘a’ and ‘s’ to increase or decrease the wheel speed. However, the biped falls on the floor immediately because the current target pose is not balanced for one-foot stance. We need to find a better target pose.

Lesson 7: Inverse kinematics

Instead of manually designing a target pose, this time we will solve for a balanced pose by formulating an inverse kinematics (IK) problem and solving it using gradient descent method. In this example, a balanced pose is defined as a pose where the center of mass is well supported by the ground contact and the left foot lies flat on the ground. As such, we cast IK as an optimization problem that minimizes the horizontal deviation between the center of mass and the center of the left foot, as well as the vertical distance of the four corners of the left foot from the ground:

where c and p indicate the projected center of mass and center of pressure on the ground, and p_i indicates the vertical height of one corner of the left foot.

To compute the gradient of the above objective function, we need to evaluate the partial derivatives of each objective term with respect to the degrees of freedom, i.e., the computation of Jacobian matrix. DART provides a comprensive set of APIs for accessing various types of Jacobian. In this example, computing the gradient of the first term of the objective function requires the Jacobian of the center of mass of the Skeleton, as well as the Jacobian of the center of mass of a BodyNode:

Eigen::VectorXd solveIK(SkeletonPtr biped)
{
...
    Eigen::Vector3d localCOM = leftHeel->getCOM(leftHeel);
    LinearJacobian jacobian = biped->getCOMLinearJacobian() - biped->getLinearJacobian(leftHeel, localCOM);
...
}

getCOMLinearJacobian returns the linear Jacobian of the center of mass of the Skeleton, while getLinearJacobian returns the Jacobian of a point on a BodyNode. The BodyNode and the local coordinate of the point are specified as parameters to this function. Here the point of interest is the center of mass of the left foot, which local coordinates can be accessed by getCOM with a parameter indicating the left foot being the frame of reference. We use getLinearJacobian again to compute the gradient of the second term of the objective function:

Eigen::VectorXd solveIK(SkeletonPtr biped)
{
...
    Eigen::Vector3d offset(0.0, -0.04, -0.03);
    gradient = biped->getLinearJacobian(leftHeel, offset).row(1);
...
}

The full list of Jacobian APIs can be found in the API Documentation. The following table summarizes the essential APIs.

This Lesson concludes the entire Biped tutorial. You should see a biped standing stably on the skateboard. With moderate acceleration/deceleration on the skateboard, the biped is able to maintain balance and hold the one-foot stance pose.