Manipulator | DART: Dynamic Animation and Robotics Toolkit

Overview

This tutorial will demonstrate some of the more advanced features of DART’s dynamics API which allow you to write robust controllers that work for real dynamic systems, such as robotic manipulators. We will show you how to:

Clone Skeletons
Load a URDF
Write a stable PD controller w/ gravity and coriolis compensation
Write an operational space controller

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

Lesson 1: Cloning Skeletons

There are often times where you might want to create an exact replica of an existing Skeleton. DART offers cloning functionality that allows you to do this very easily.

Lesson 1a: Create a new domino

Creating a new domino is straightforward. Find the function attemptToCreateDomino in the MyWindow class. The class has a member called mFirstDomino which is the original domino created when the program starts up. To make a new one, we can just clone it:

SkeletonPtr newDomino = mFirstDomino->clone();

But keep in mind that every Skeleton that gets added to a world requires its own unique name. Creating a clone will keep the original name, so we should we give the new copy its own name:

newDomino->setName("domino #" + std::to_string(mDominoes.size() + 1));

So the easy part is finished, but now we need to get the domino to the correct position. First, let’s grab the last domino that was placed in the environment:

const SkeletonPtr& lastDomino = mDominoes.size() > 0 ?
      mDominoes.back() : mFirstDomino;

Now we should compute what we want its position to be. The MyWindow class keeps a member called mTotalAngle which tracks how much the line of dominoes has turned so far. We’ll use that to figure out what translational offset the new domino should have from the last domino:

Eigen::Vector3d dx = default_distance * Eigen::Vector3d(
      cos(mTotalAngle), sin(mTotalAngle), 0.0);

And now we can compute the total position of the new domino. First, we’ll copy the positions of the last domino:

Eigen::Vector6d x = lastDomino->getPositions();

And then we’ll add the translational offset to it:

x.tail<3>() += dx;

Remember that the domino’s root joint is a FreeJoint which has six degrees of freedom: the first three are for orientation and last three are for translation.

Finally, we should add on the change in angle for the new domino:

x[2] = mTotalAngle + angle;

Be sure to uncomment the angle argument of the function.

Now we can use x to set the positions of the domino:

newDomino->setPositions(x);

The root FreeJoint is the only joint in the domino’s Skeleton, so we can just use the Skeleton::setPositions function to set it.

Lesson 1b: Make sure no dominoes are in collision

Similar to Lesson 3 of the Collisions tutorial, we’ll want to make sure that the newly inserted Skeleton is not starting out in collision with anything, because this could make for a very ugly (perhaps even broken) simulation.

First, we get the CollisionGroup with the ShapeFrames of the things in the world

auto collisionGroup = mWorld->getConstraintSolver()->getCollisionGroup();

Then we’ll create a second CollisionGroup which only contains the new domino.

auto collisionEngine = mWorld->getConstraintSolver()->getCollisionDetector();
auto newGroup = collisionEngine->createCollisionGroup(newDomino.get());

Then we’ll remove the floor from the first collision group. We ignore collisions with the floor because, mathematically speaking, if they are in contact with the floor then they register as being in collision. But we want the dominoes to be in contact with the floor, so this is okay. After that the collision detector computes if there is a collision between the new domino and the existing things. Then the floor is added back to the group of collision frames in the world.

collisionGroup->removeShapeFramesOf(mFloor.get());
bool dominoCollision = collisionGroup->collide(newGroup.get());
collisionGroup->addShapeFramesOf(mFloor.get());

If the new domino would collide with the existing things in the world except the floor, then we will not add it to the world.

if(dominoCollision)
{
  std::cout << "The new domino would collide with something!"    << std::endl;
}

Otherwise, if the new domino is in an okay position, we should add it to the world and to history:

else
{
  mWorld->addSkeleton(newDomino);
  // Record the latest domino addition
  mAngles.push_back(angle);
  mDominoes.push_back(newDomino);
  mTotalAngle += angle;
}

Lesson 1c: Delete the last domino added

Ordinarily, removing a Skeleton from a scene is just a matter of calling the World::removeSkeleton function, but we have a little bit of bookkeeping to take care of for our particular application. First, we should check whether there are any dominoes to actually remove:

if(mDominoes.size() > 0)
{
  // TODO: Remove Skeleton
}

Then we should grab the last domino in the history, remove it from the history, and then take it out of the world:

SkeletonPtr lastDomino = mDominoes.back();
mDominoes.pop_back();
mWorld->removeSkeleton(lastDomino);

The SkeletonPtr class is really a std::shared_ptr<Skeleton> so we don’t need to worry about ever calling delete on it. Instead, its resources will be freed when lastDomino goes out of scope.

We should also make sure to do the bookkeepping for the angles:

mTotalAngle -= mAngles.back();
mAngles.pop_back();

Now we can add and remove dominoes from the scene. Feel free to give it a try.

Lesson 1d: Apply a force to the first domino

But just setting up dominoes isn’t much fun without being able to knock them down. We can quickly and easily knock down the dominoes by magically applying a force to the first one. In the timeStepping function of MyWindow there is a label for Lesson 1d. This spot will get visited whenever the user presses ‘f’, so we’ll apply an external force to the first domino here:

Eigen::Vector3d force = default_push_force * Eigen::Vector3d::UnitX();
Eigen::Vector3d location =
    default_domino_height / 2.0 * Eigen::Vector3d::UnitZ();
mFirstDomino->getBodyNode(0)->addExtForce(force, location);

Lesson 2: Loading and controlling a robotic manipulator

Striking something with a magical force is convenient, but not very believable. Instead, let’s load a robotic manipulator and have it push over the first domino.

Lesson 2a: Load a URDF file

Our manipulator is going to be loaded from a URDF file. URDF files are loaded by the dart::utils::DartLoader class (pending upcoming changes to DART’s loading system). First, create a loader:

dart::utils::DartLoader loader;

Note that many URDF files use ROS’s package: scheme to specify the locations of the resources that need to be loaded. We won’t be using this in our example, but in general you should use the function DartLoader::addPackageDirectory to specify the locations of these packages, because DART does not have the same package resolving abilities of ROS.

Now we’ll have loader parse the file into a Skeleton:

SkeletonPtr manipulator =
    loader.parseSkeleton(DART_DATA_PATH"urdf/KR5/KR5 sixx R650.urdf");

And we should give the Skeleton a convenient name:

manipulator->setName("manipulator");

Now we’ll want to initialize the location and configuration of the manipulator. Experimentation has demonstrated that the following setup is good for our purposes:

// Position its base in a reasonable way
Eigen::Isometry3d tf = Eigen::Isometry3d::Identity();
tf.translation() = Eigen::Vector3d(-0.65, 0.0, 0.0);
manipulator->getJoint(0)->setTransformFromParentBodyNode(tf);

// Get it into a useful configuration
manipulator->getDof(1)->setPosition(140.0 * M_PI / 180.0);
manipulator->getDof(2)->setPosition(-140.0 * M_PI / 180.0);

And lastly, be sure to return the Skeleton that we loaded rather than the dummy Skeleton that was originally there:

return manipulator;

Feel free to load up the application to see the manipulator in the scene, although all it will be able to do is collapse pitifully onto the floor.

Lesson 2b: Grab the desired joint angles

To make the manipulator actually useful, we’ll want to have the Controller control its joint forces. For it to do that, the Controller class will need to be informed of what we want the manipulator’s joint angles to be. This is easily done in the constructor of the Controller class:

mQDesired = mManipulator->getPositions();

The function Skeleton::getPositions will get all the generalized coordinate positions of all the joints in the Skeleton, stacked in a single vector. These Skeleton API functions are useful when commanding or controlling an entire Skeleton with a single mathematical expression.

Lesson 2c: Write a stable PD controller for the manipulator

Now that we know what configuration we want the manipulator to hold, we can write a PD controller that keeps them in place. Find the function setPDForces in the Controller class.

First, we’ll grab the current positions and velocities:

Eigen::VectorXd q = mManipulator->getPositions();
Eigen::VectorXd dq = mManipulator->getVelocities();

Additionally, we’ll integrate the position forward by one timestep:

q += dq * mManipulator->getTimeStep();

This is not necessary for writing a regular PD controller, but instead this is to write a “stable PD” controller which has some better numerical stability properties than an ordinary discrete PD controller. You can try running with and without this line to see what effect it has on the stability.

Now we’ll compute our joint position error:

Eigen::VectorXd q_err = mQDesired - q;

And our joint velocity error, assuming our desired joint velocity is zero:

Eigen::VectorXd dq_err = -dq;

Now we can grab our mass matrix, which we will use to scale our force terms:

const Eigen::MatrixXd& M = mManipulator->getMassMatrix();

And then combine all this into a PD controller that computes forces to minimize our error:

mForces = M * (mKpPD * q_err + mKdPD * dq_err);

Now we’re ready to set these forces on the manipulator:

mManipulator->setForces(mForces);

Feel free to give this PD controller a try to see how effective it is.

Lesson 2d: Compensate for gravity and Coriolis forces

One of the key features of DART is the ability to easily compute the gravity and Coriolis forces, allowing you to write much higher quality controllers than you would be able to otherwise. This is easily done like so:

const Eigen::VectorXd& Cg = mManipulator->getCoriolisAndGravityForces();

And now we can update our control law by just slapping this term onto the end of the equation:

mForces = M * (mKpPD * q_err + mKdPD * dq_err) + Cg;

Give this new PD controller a try to see how its performance compares to the one without compensation

Lesson 3: Writing an operational space controller

While PD controllers are simply and handy, operational space controllers can be much more elegant and useful for performing tasks. Operational space controllers allow us to unify geometric tasks (like getting the end effector to a particular spot) and dynamics tasks (like applying a certain force with the end effector) all while remaining stable and smooth.

Lesson 3a: Set up the information needed for an OS controller

Unlike PD controllers, an operational space controller needs more information than just desired joint angles.

First, we’ll grab the last BodyNode on the manipulator and treat it as an end effector:

mEndEffector = mManipulator->getBodyNode(mManipulator->getNumBodyNodes() - 1);

But we don’t want to use the origin of the BodyNode frame as the origin of our Operational Space controller; instead we want to use a slight offset, to get to the tool area of the last BodyNode:

mOffset = default_endeffector_offset * Eigen::Vector3d::UnitX();

Also, our target will be the spot on top of the first domino, so we’ll create a reference frame and place it there. First, create the SimpleFrame:

mTarget = std::make_shared<SimpleFrame>(Frame::World(), "target");

Then compute the transform needed to get from the center of the domino to the top of the domino:

Eigen::Isometry3d target_offset(Eigen::Isometry3d::Identity());
target_offset.translation() =
    default_domino_height / 2.0 * Eigen::Vector3d::UnitZ();

And then we should rotate the target’s coordinate frame to make sure that lines up with the end effector’s reference frame, otherwise the manipulator might try to push on the domino from a very strange angle:

target_offset.linear() =
    mEndEffector->getTransform(domino->getBodyNode(0)).linear();

Now we’ll set the target so that it has a transform of target_offset with respect to the frame of the domino:

mTarget->setTransform(target_offset, domino->getBodyNode(0));

And this gives us all the information we need to write an Operational Space controller.

Lesson 3b: Computing forces for OS Controller

Find the function setOperationalSpaceForces(). This is where we’ll compute the forces for our operational space controller.

One of the key ingredients in an operational space controller is the mass matrix. We can get this easily, just like we did for the PD controller:

const Eigen::MatrixXd& M = mManipulator->getMassMatrix();

Next we’ll want the Jacobian of the tool offset in the end effector. We can get it easily with this function:

Jacobian J = mEndEffector->getWorldJacobian(mOffset);

But operational space controllers typically use the Moore-Penrose pseudoinverse of the Jacobian rather than the Jacobian itself. There are many ways to compute the pseudoinverse of the Jacobian, but a simple way is like this:

Eigen::MatrixXd pinv_J = J.transpose() * (J * J.transpose()
                       + 0.0025 * Eigen::Matrix6d::Identity()).inverse();

Note that this pseudoinverse is also damped so that it behaves better around singularities. This is method for computing the pseudoinverse is not very efficient in terms of the number of mathematical operations it performs, but it is plenty fast for our application. Consider using methods based on Singular Value Decomposition if you need to compute the pseudoinverse as fast as possible.

Next we’ll want the time derivative of the Jacobian, as well as its pseudoinverse:

// Compute the Jacobian time derivative
Jacobian dJ = mEndEffector->getJacobianClassicDeriv(mOffset);

// Comptue the pseudo-inverse of the Jacobian time derivative
Eigen::MatrixXd pinv_dJ = dJ.transpose() * (dJ * dJ.transpose()
                        + 0.0025 * Eigen::Matrix6d::Identity()).inverse();

Notice that here we’re compute the classic derivative, which means the derivative of the Jacobian with respect to time in classical coordinates rather than spatial coordinates. If you use spatial vector arithmetic, then you’ll want to use BodyNode::getJacobianSpatialDeriv instead.

Now we can compute the linear components of error:

Eigen::Vector6d e;
e.tail<3>() = mTarget->getWorldTransform().translation()
            - mEndEffector->getWorldTransform() * mOffset;

And then the angular components of error:

Eigen::AngleAxisd aa(mTarget->getTransform(mEndEffector).linear());
e.head<3>() = aa.angle() * aa.axis();

Then the time derivative of error, assuming our desired velocity is zero:

Eigen::Vector6d de = -mEndEffector->getSpatialVelocity(
      mOffset, mTarget.get(), Frame::World());

Like with the PD controller, we can mix in terms to compensate for gravity and Coriolis forces:

const Eigen::VectorXd& Cg = mManipulator->getCoriolisAndGravityForces();

The gains for the operational space controller need to be in matrix form, but we’re storing the gains as scalars, so we’ll need to conver them:

Eigen::Matrix6d Kp = mKpOS * Eigen::Matrix6d::Identity();

size_t dofs = mManipulator->getNumDofs();
Eigen::MatrixXd Kd = mKdOS * Eigen::MatrixXd::Identity(dofs, dofs);

And we’ll need to compute the joint forces needed to achieve our desired end effector force. This is easily done using the Jacobian transpose:

Eigen::Vector6d fDesired = Eigen::Vector6d::Zero();
fDesired[3] = default_push_force;
Eigen::VectorXd f = J.transpose() * fDesired;

And now we can mix everything together into the single control law:

Eigen::VectorXd dq = mManipulator->getVelocities();
mForces = M * (pinv_J * Kp * de + pinv_dJ * Kp * e)
          - Kd * dq + Kd * pinv_J * Kp * e + Cg + f;

Then don’t forget to pass the forces into the manipulator:

mManipulator->setForces(mForces);

Now you’re ready to try out the full dominoes app!