Using DART to control a robotic manipulator

Uni­ver­sal Ro­bot De­scrip­tion Format

Nev­er­the­less, these are still math­em­at­ic­al for­mu­las and they re­quire quite some work to manu­ally per­form all the com­pu­ta­tions. DART (and most ro­bot­ic sim­u­lat­ors) already have some of the two con­ven­tions im­ple­men­ted and re­quire from the user to give only the re­l­at­ive trans­form­a­tion from each link to their “par­ent” joints and from the joints to their “child” links. This greatly sim­pli­fies the life of the user, but still writ­ing this in C++ code is de­mand­ing and er­ror prone (just have a look at the code above to cre­ate a simple box). For this reas­on, sev­er­al de­scrip­tion formats have arised over the years that try to make it easi­er and more in­tu­it­ive to cre­ate your ro­bots.

One of the most pop­u­lar formats is the Uni­ver­sal Ro­bot De­scrip­tion Format (URDF) that is part of the Ro­bot­ics Op­er­at­ing Sys­tem (ROS) frame­work. URDF is an XML format for rep­res­ent­ing a ro­bot mod­el; for more de­tails, please have a look at the ROS wiki. Here is how a part of the URDF mod­el of the ma­nip­u­lat­or that we are go­ing to use looks like:

<!-- link 0 -->
<link name="iiwa_link_0">
  <inertial>
    <origin rpy="0 0 0" xyz="-0.1 0 0.07"/>
    <mass value="5"/>
    <inertia ixx="0.05" ixy="0" ixz="0" iyy="0.06" iyz="0" izz="0.03"/>
  </inertial>
  <visual>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <geometry>
      <mesh filename="package://iiwa14/link_0.stl"/>
    </geometry>
    <material name="Grey"/>
  </visual>
  <collision>
    <origin rpy="0 0 0" xyz="-0.01 0 0.07875"/>
    <geometry>
      <box size="0.24 0.23 0.1575"/>
    </geometry>
    <material name="Grey"/>
  </collision>
</link>
<!-- joint between link 0 and link 1 -->
<joint name="iiwa_joint_1" type="revolute">
  <parent link="iiwa_link_0"/>
  <child link="iiwa_link_1"/>
  <origin rpy="0 0 0" xyz="0 0 0.1575"/>
  <axis xyz="0 0 1"/>
  <limit effort="320" lower="-2.96705972" upper="2.96705972" velocity="1.4835298"/>
  <dynamics damping="0.5"/>
</joint>
<!-- link 1 -->
<link name="iiwa_link_1">
  ...
</link>

This is to get an idea of how a URDF file would look like; no need to un­der­stand it!

Load­ing a URDF mod­el with DART

As­sum­ing that we have in our pos­ses­sion the URDF mod­el of our ro­bot, here is how to load it with DART:

/* DART: Load Skeletons/Robots */
DartLoader loader;
/* Add packages (needed for URDF loading): this is a ROS-related thing in
   order to find the resources (e.g., meshes), see the "package://" in the
   URDF file above */
loader.addPackageDirectory("name_of_package", "path/to/package");
std::string filename = "path/to/URDF/file";

/* Load the URDF in a DART Skeleton */
auto manipulator = loader.parseSkeleton(filename);

The KUKA LBR Ii­wa ma­nip­u­lat­or URDF that we used is a mod­i­fied ver­sion of the one in the ii­wa_ros pack­age. The Ro­botiq grip­per 2F-85 URDF that we used is a mod­i­fied ver­sion of the one in the ro­botiq_ar­g85_­de­scrip­tion pack­age. Once we have loaded/cre­ated all our ro­bots, we add them to the DART world:

/* Add the robot/objects in our DART world */
world->addSkeleton(manipulator);
world->addSkeleton(floorSkel);
world->addSkeleton(redBoxSkel);
world->addSkeleton(greenBoxSkel);
world->addSkeleton(blueBoxSkel);

How does DART con­nect to Mag­num?

DART con­nects to Mag­num through the SceneGraph lib­rary, sim­il­ar to how Bul­letInteg­ra­tion does it. DART in­teg­ra­tion provides two main classes: (a) DartInteg­ra­tion::World, and (b) DartInteg­ra­tion::Ob­ject. The most com­mon us­age will be some­thing like the fol­low­ing:

/* DART world */
dart::simulation::WorldPtr dartWorld{new dart::simulation::World};

// ... add robots and objects into DART world ...

/* Create our DartIntegration object/world */
auto dartObj = new Object3D{&_scene};
auto world = std::make_unique<DartIntegration::World>(*dartObj, *dartWorld));

/* Simulate with time step of 0.001 seconds */
world.world().setTimeStep(0.001);

for(UnsignedInt i = 0; i < simulationSteps; ++i) {
    world.step();

    /* Update graphics at ~60Hz (15*0.001 ~= 60Hz) */
    if(i % 15 == 0) {
        world.refresh();

        /* Get unused/deleted shapes */
        std::vector<std::unique_ptr<DartIntegration::Object>>& unusedObjects
            = world.unusedObjects();

        /* The user is expected to handle unused objects. One possible
           handling would be to remove them from the parent scene. */
        deleteObjectsFromScene(unusedObjects);

        /* Get updated shapes -- ones that either the materials or the
           meshes have changed */
        std::vector<std::reference_wrapper<DartIntegration::Object>>
            updatedObjects = world.updatedShapeObjects();

        updateMeshesAndMaterials(updatedObjects);

        /* Clear list of updated objects */
        world.clearUpdatedShapeObjects();
    }
}

In es­sence, the user needs to handle the de­leted ob­jects and the ones that need to be up­dated. In our ex­ample, noth­ing should be de­leted and thus we do not handle the de­leted ob­jects. In or­der to draw the meshes of our ro­bots, we need to cre­ate a struc­ture. We will as­sume a Phong shader and cre­ate the fol­low­ing class:

class DrawableObject: public Object3D, SceneGraph::Drawable3D {
    public:
        explicit DrawableObject(
            const std::vector<Containers::Reference<GL::Mesh>>& meshes,
            const std::vector<MaterialData>& materials, Object3D* parent,
            SceneGraph::DrawableGroup3D* group);

        DrawableObject& setMeshes(
            const std::vector<Containers::Reference<GL::Mesh>>& meshes);
        DrawableObject& setMaterials(
            const std::vector<MaterialData>& materials);
        DrawableObject& setSoftBodies(const std::vector<bool>& softBody);
        DrawableObject& setTextures(std::vector<GL::Texture2D*>& textures);

    private:
        void draw(const Matrix4& transformationMatrix, SceneGraph::Camera3D& camera)
            override;

        Resource<Shaders::Phong> _colorShader;
        Resource<Shaders::Phong> _textureShader;
        std::vector<Containers::Reference<GL::Mesh>> _meshes;
        std::vector<MaterialData> _materials;
        std::vector<bool> _isSoftBody;
        std::vector<GL::Texture2D*> _textures;
};

Note that each DartInteg­ra­tion::Ob­ject can con­tain mul­tiple meshes with col­or or tex­ture ma­ter­i­al. To keep track of which ob­jects are be­ing up­dated (this should only hap­pen if the visu­al prop­er­ties, that is the mesh or ma­ter­i­al in­form­a­tion, of a body changes), we have defined a std::un­ordered_map:

std::unordered_map<DartIntegration::Object*, DrawableObject*> _drawableObjects;

To up­date the in­form­a­tion of our ob­jects (both the trans­form­a­tions but also the graph­ics part), we per­form the fol­low­ing:

world->refresh();

To get and up­date the new meshes and ma­ter­i­al in­form­a­tion, we per­form some­thing like the fol­low­ing:

/* For each updated object */
for(DartIntegration::Object& object: world->updatedShapeObjects()) {
    /* Get material information */
    std::vector<MaterialData> materials;
    std::vector<Containers::Reference<GL::Mesh>> meshes;
    std::vector<bool> isSoftBody;
    std::vector<L::Texture2D*> textures;
    for(std::size_t i = 0; i < object.drawData().meshes.size(); i++) {
        bool isColor = true;
        GL::Texture2D* texture = nullptr;
        if(object.drawData().materials[i].flags() &
            Trade::PhongMaterialData::Flag::DiffuseTexture)
        {
            Containers::Optional<GL::Texture2D>& entry = object.drawData()
                .textures[object.drawData().materials[i].diffuseTexture()];
            if(entry) {
                texture = &*entry;
                isColor = false;
            }
        }

        textures.push_back(texture);

        MaterialData mat;
        mat.ambientColor = object.drawData().materials[i].ambientColor().rgb();
        if(isColor)
            mat.diffuseColor = object.drawData().materials[i].diffuseColor().rgb();
        mat.specularColor = object.drawData().materials[i].specularColor().rgb();
        mat.shininess = object.drawData().materials[i].shininess();
        mat.scaling = object.drawData().scaling;

        /* Get the modified mesh */
        meshes.push_back(object.drawData().meshes[i]);
        materials.push_back(mat);
        isSoftBody.push_back(object.shapeNode()->getShape()->getType() ==
            dart::dynamics::SoftMeshShape::getStaticType());
    }

    /* Check if we already have it and then either add a new one or update
       the existing. We don't need the mesh / material / texture vectors
       anywhere else anymore, so move them in to avoid copies. */
    auto it = _drawableObjects.insert(std::make_pair(&object, nullptr));
    if(it.second) {
        auto drawableObj = new DrawableObject{
            std::move(meshes), std::move(materials),
            static_cast<Object3D*>(&(object.object())), &_drawables};
        drawableObj->setSoftBodies(std::move(isSoftBody));
        drawableObj->setTextures(std::move(textures));
        it.first->second = drawableObj;
    } else {
        (*it.first->second)
            .setMeshes(std::move(meshes))
            .setMaterials(std::move(materials))
            .setSoftBodies(std::move(isSoftBody))
            .setTextures(std::move(textures));
    }
}

world->clearUpdatedShapeObjects();

The DartInteg­ra­tion::Ob­ject stores in memory the mesh, ma­ter­i­al and tex­ture in­form­a­tion and thus it is ad­vised to keep ref­er­ences or point­ers to these val­ues. Put­ting everything to­geth­er and with a few ad­di­tion­al things (e.g., two lights), we can get some­thing like the fol­low­ing:

Sim­u­lat­ing any ro­bot with the DartInteg­ra­tion

Us­ing the ex­actly the same code (just chan­ging the URDF file), we can load any ro­bot in DART and render it with Mag­num. Here are a few oth­er ex­amples ren­der­ing a hu­manoid (iCub), a hexa­pod and quad­ruped (ANY­mal) ro­bot:

Ac­tu­at­or types

Ro­bots are mech­an­ic­al struc­tures that are in­ter­con­nec­ted and con­trolled via elec­tric­al com­pon­ents. This means that the low­est level con­trol sig­nal hap­pens in cur­rent. Be­cause hand­ling cur­rent is dif­fi­cult and non-in­tu­it­ive, there are two types of mo­tors widely used to provide a high­er level of ab­strac­tion:

• Torque-con­trolled ac­tu­at­ors; these ac­tu­at­ors are equipped with a force/torque sensor and the high­er level con­trol sig­nals are the de­sired torques/forces (de­pend­ing on the type of the mech­an­ic­al con­nec­tion).
• Servo ac­tu­at­ors; these ac­tu­at­ors are equipped with an en­coder sensor (that meas­ures the dis­place­ment or cur­rent po­s­i­tion of the joint) and the high­er level con­trol sig­nals are the de­sired joint po­s­i­tions.

Con­trolling in joint space

The easi­est type of con­trol is to con­trol the ro­bot in what we call the joint-space. This ba­sic­ally means that we have some de­sired joint po­s­i­tions and some de­sired joint ve­lo­cit­ies (usu­ally set to zero) and we want to find the con­trol sig­nal to achieve them.

When con­trolling in joint-space, the most clas­sic­al con­trol­ler is a Pro­por­tion­al–In­teg­ral–De­riv­at­ive (PID) con­trol­ler. In es­sence, the de­sired con­trol sig­nal (no mat­ter the type) is com­puted as fol­lows:

The user here will need to tune the para­met­ers . In ro­bot­ics, a vari­ation of this con­trol is widely used (usu­ally named PD-con­trol­ler):

Us­ing these simple con­trol­lers, one can do some very amaz­ing and in­ter­est­ing things.

Con­trolling in task space

How­ever, most of the in­ter­est­ing things hap­pen in the Cartesian 6D space. This is the space where the end-ef­fect­or of our ma­nip­u­lat­or lives in. Moreover, this is the space where most of the real world tasks can be very in­tu­it­ively de­scribed. For this reas­on, we call it the task-space.

/* Get joint velocities of manipulator */
Eigen::VectorXd dq = _model->getVelocities();

/* Get full (with orientation) Jacobian of our end-effector */
Eigen::MatrixXd J = _model->getBodyNode("iiwa_link_ee")->getWorldJacobian();

/* Get current state of the end-effector */
Eigen::MatrixXd currentWorldTransformation = _model->getBodyNode("iiwa_link_ee")
    ->getWorldTransform().matrix();
Eigen::VectorXd currentWorldPosition =
    currentWorldTransformation.block(0, 3, 3, 1);
Eigen::MatrixXd currentWorldOrientation =
    currentWorldTransformation.block(0, 0, 3, 3);
Eigen::VectorXd currentWorldSpatialVelocity = _model->getBodyNode("iiwa_link_ee")
    ->getSpatialVelocity(dart::dynamics::Frame::World(),
                         dart::dynamics::Frame::World());

/* Compute desired forces and torques */
Eigen::VectorXd linearError = _desiredPosition - currentWorldPosition;
/* PD controller in end-effector position */
Eigen::VectorXd desiredForces = _pLinearGain*linearError -
    _dLinearGain*currentWorldSpatialVelocity.tail(3);

/* Special care needs to be taken to compute orientation differences.
   We use the angle-axis representation */
Eigen::VectorXd orientationError =
    dart::math::logMap(_desiredOrientation*currentWorldOrientation.transpose());
/* PD controller in end-effector orientation */
Eigen::VectorXd desiredTorques = _pOrientationGain*orientationError -
    _dOrientationGain*currentWorldSpatialVelocity.head(3);

/* Combine forces and torques in one vector */
Eigen::VectorXd tau(6);
tau.head(3) = desiredTorques;
tau.tail(3) = desiredForces;

/* Compute final forces + gravity compensation + regularization */
forces = J.transpose()*tau + _model->getCoriolisAndGravityForces() -
    _dRegularization*dq;

/* Send commands to the robot */
_model->setCommands(forces);