A new ex­am­ple show­cas­es ca­pa­bil­i­ties of the DART in­te­gra­tion. Let’s dive in­to ro­bot­ics, ex­plain what DART is able to do and how it com­pares to Bul­let.

What is a ro­bot­ic ma­nip­u­la­tor?

A ro­botic mech­a­nism is con­struct­ed by con­nect­ing rigid bod­ies, called links, to­geth­er by means of joints, so that rel­a­tive mo­tion be­tween ad­ja­cent links be­comes pos­si­ble. Ac­tu­a­tion of the joints, typ­i­cal­ly by elec­tric mo­tors, then caus­es the ro­bot to move and ex­ert forces in de­sired ways. Ad­di­tion­al­ly, there ex­ist sev­er­al sen­sors that al­lows us to read the cur­rent po­si­tion, ve­loc­i­ty or torque of an ac­tu­a­tor.

A ro­botic ma­nip­u­la­tor is a ro­botic mech­a­nism where its links are ar­ranged in se­ri­al fash­ion, that is, they form what we call an open-chain. With­out go­ing very deeply in­to the me­chan­i­cal de­tails be­hind the ac­tu­al ma­chines and with­out loss of gen­er­al­i­ty, for the re­main­ing of this post a ro­botic ma­nip­u­la­tor can be de­scribed ful­ly by the mass and in­er­tia prop­er­ties of its links, its kine­mat­ic con­fig­u­ra­tion, its joint po­si­tions and its joint ve­loc­i­ties. More­over, we will as­sume that our ma­nip­u­la­tors can be con­trol ei­ther via torque or ve­loc­i­ty com­mands.

What is this ex­am­ple about?

Be­fore mov­ing on, let us see what we can do in this ex­am­ple (and what we will ex­plain in this post):

As we can see in the video, our ex­am­ple world con­sists of a ro­botic ma­nip­u­la­tor with a grip­per at­tached in its end-ef­fec­tor (that is, the last link in the se­ri­al chain) and three small col­ored box­es. The us­er can con­trol the ma­nip­u­la­tor with in­tu­itive com­mands (e.g., move over the red box or move up/down) and pick up the small box­es to cre­ate some tow­ers. They can even de­stroy the ones that they cre­at­ed!

We will split the post in­to two main di­rec­tions: (a) ex­plain­ing how to use the Dart­In­te­gra­tion (and some ba­sic us­age of DART), and (b) ex­plain­ing the ba­sics of the con­troller de­vel­oped for this task (from a ro­bot­ics per­spec­tive).

Us­ing the Dart­In­te­gra­tion

Dy­nam­ic An­i­ma­tion and Ro­bot­ics Tool­kit (DART) is an open-source li­brary that pro­vides da­ta struc­tures and al­go­rithms for kine­mat­ic and dy­nam­ic ap­pli­ca­tions in ro­bot­ics and com­put­er an­i­ma­tion. Since its main fo­cus is ro­bot­ics ap­pli­ca­tions, ev­ery en­ti­ty in a DART world is a kine­mat­ic skele­ton, that is, a col­lec­tion of links in­ter­con­nect­ed with joints. Each joint can be ac­tu­at­ed with dif­fer­ent types of ac­tu­a­tion (force or torque, ve­loc­i­ty, po­si­tion) or it can be fixed. DART ad­di­tion­al­ly sup­port non-rigid bod­ies/links, but we will on­ly con­sid­er ro­bots with rigid body links in this post. In DART ev­ery­thing be­gins by cre­at­ing an emp­ty world:

auto world = dart::simulation::WorldPtr(new dart::simulation::World);

In­side this imag­i­nary world, our ro­bots will live and flour­ish. The next step is to cre­ate our ro­bots. In DART, even the small box­es that have no ac­tu­a­tion are con­sid­ered as a kine­mat­ic skele­ton. Let us cre­ate our fist “ro­bot” box (for more de­tails on how to use DART, please fol­low their tu­to­ri­als):

/* The size of our box */
constexpr Double boxSize = 0.06;

/* Calculate the mass of the box */
constexpr Double boxDensity = 260; // kg/m^3
constexpr Double boxMass = boxDensity*Math::pow<3>(boxSize);

/* Create a Skeleton and give it a name */
dart::dynamics::SkeletonPtr box = dart::dynamics::Skeleton::create("box");

/* Create a body for the box */
dart::dynamics::BodyNodePtr body =
    box->createJointAndBodyNodePair<dart::dynamics::FreeJoint>(nullptr).second;

/* Create a shape for the box */
auto boxShape = std::make_shared<dart::dynamics::BoxShape>(
    Eigen::Vector3d{box_size, box_size, box_size});
auto shapeNode = body->createShapeNodeWith<
    dart::dynamics::VisualAspect,
    dart::dynamics::CollisionAspect, dart::dynamics::DynamicsAspect>(box_shape);

/* Give a color to our box */
shapeNode->getVisualAspect()->setColor(Eigen::Vector3d{1.0, 0.0, 0.0});

/* Set up inertia for the box */
dart::dynamics::Inertia inertia;
inertia.setMass(box_mass);
inertia.setMoment(box_shape->computeInertia(box_mass));
body->setInertia(inertia);

/* Setup the center of the box properly */
box->getDof("Joint_pos_z")->setPosition(box_size/2.0);

See this code in the mag­num-ex­am­ples re­pos­i­to­ry.

The are sev­er­al steps that we need to take in or­der to cre­ate a ro­bot (in DART but in gen­er­al al­so). Imag­ine if we have to cre­ate a ro­bot with mul­ti­ple links and joints! For that rea­son, sev­er­al con­ven­tions have been de­fined and used in the ro­bot­ics com­mu­ni­ty. The most pop­u­lar ones are the De­nav­it–Harten­berg pa­ram­e­ters and the prod­uct of ex­po­nen­tials for­mu­la (that is based on the screw the­o­ry; for more in­for­ma­tion please re­fer to the Mod­ern Ro­bot­ics book by Kevin M. Lynch and Frank C. Park).

Uni­ver­sal Ro­bot De­scrip­tion For­mat

Nev­er­the­less, these are still math­e­mat­i­cal for­mu­las and they re­quire quite some work to man­u­al­ly per­form all the com­pu­ta­tions. DART (and most ro­botic sim­u­la­tors) al­ready have some of the two con­ven­tions im­ple­ment­ed and re­quire from the us­er to give on­ly the rel­a­tive trans­for­ma­tion from each link to their “par­ent” joints and from the joints to their “child” links. This great­ly sim­pli­fies the life of the us­er, 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 sim­ple box). For this rea­son, sev­er­al de­scrip­tion for­mats have arised over the years that try to make it eas­i­er and more in­tu­itive to cre­ate your ro­bots.

One of the most pop­u­lar for­mats is the Uni­ver­sal Ro­bot De­scrip­tion For­mat (URDF) that is part of the Ro­bot­ics Op­er­at­ing Sys­tem (ROS) frame­work. URDF is an XML for­mat for rep­re­sent­ing a ro­bot mod­el; for more de­tails, please have a look at the ROS wi­ki. Here is how a part of the URDF mod­el of the ma­nip­u­la­tor 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>

See the full file in the mag­num-ex­am­ples re­pos­i­to­ry.

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);

See this code in the mag­num-ex­am­ples re­pos­i­to­ry.

The KU­KA LBR Ii­wa ma­nip­u­la­tor URDF that we used is a mod­i­fied ver­sion of the one in the ii­wa_ros pack­age. The Robo­tiq grip­per 2F-85 URDF that we used is a mod­i­fied ver­sion of the one in the robo­tiq_arg85_de­scrip­tion pack­age. Once we have load­ed/cre­at­ed 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);

See this code in the mag­num-ex­am­ples re­pos­i­to­ry.

How does DART con­nect to Mag­num?

DART con­nects to Mag­num through the Scene­Graph li­brary, sim­i­lar to how Bul­let­Inte­gra­tion does it. DART in­te­gra­tion pro­vides two main class­es: (a) Dart­In­te­gra­tion::World, and (b) Dart­In­te­gra­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 essence, the us­er needs to han­dle the delet­ed ob­jects and the ones that need to be up­dat­ed. In our ex­am­ple, noth­ing should be delet­ed and thus we do not han­dle the delet­ed ob­jects. In or­der to draw the mesh­es of our ro­bots, we need to cre­ate a struc­ture. We will as­sume a Phong shad­er 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;
};

See this code in the mag­num-ex­am­ples re­pos­i­to­ry.

Note that each Dart­In­te­gra­tion::Ob­ject can con­tain mul­ti­ple mesh­es with col­or or tex­ture ma­te­ri­al. To keep track of which ob­jects are be­ing up­dat­ed (this should on­ly hap­pen if the vis­ual prop­er­ties, that is the mesh or ma­te­ri­al in­for­ma­tion, of a body changes), we have de­fined a std::un­or­dered_map:

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

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

world->refresh();

To get and up­date the new mesh­es and ma­te­ri­al in­for­ma­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();

See this code in the mag­num-ex­am­ples re­pos­i­to­ry.

The Dart­In­te­gra­tion::Ob­ject stores in mem­o­ry the mesh, ma­te­ri­al and tex­ture in­for­ma­tion and thus it is ad­vised to keep ref­er­ences or point­ers to these val­ues. Putting ev­ery­thing 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 Dart­In­te­gra­tion

Us­ing the ex­act­ly the same code (just chang­ing the URDF file), we can load any ro­bot in DART and ren­der it with Mag­num. Here are a few oth­er ex­am­ples ren­der­ing a hu­manoid (iCub), a hexa­pod and quadruped (ANY­mal) ro­bot:

Con­trol­ling a ro­bot­ic ma­nip­u­la­tor

The ex­am­ple scene looks (rel­a­tive­ly) nice, but up un­til now we can­not con­trol what hap­pens to our imag­i­nary world. But most of the fun comes when we can con­trol our ro­bots and make them do things. In this sec­ond part of the post, we will see how we can con­trol our ma­nip­u­la­tor in or­der to be able to give it in­tu­itive com­mands to per­form some ev­ery­day tasks.

As we have al­ready dis­cussed, our ro­bot’s state can be de­scribed by its joint po­si­tions and its joint ve­loc­i­ties. In the ro­bot­ics lit­er­a­ture, we find the no­ta­tion q for joint po­si­tions and \dot{q} for joint ve­loc­i­ties.

Ac­tu­a­tor types

Ro­bots are me­chan­i­cal struc­tures that are in­ter­con­nect­ed and con­trolled via elec­tri­cal com­po­nents. This means that the low­est lev­el con­trol sig­nal hap­pens in cur­rent. Be­cause han­dling cur­rent is dif­fi­cult and non-in­tu­itive, there are two types of mo­tors wide­ly used to pro­vide a high­er lev­el of ab­strac­tion:

  • Torque-con­trolled ac­tu­a­tors; these ac­tu­a­tors are equipped with a force/torque sen­sor and the high­er lev­el con­trol sig­nals are the de­sired torques/forces (de­pend­ing on the type of the me­chan­i­cal con­nec­tion).
  • Ser­vo ac­tu­a­tors; these ac­tu­a­tors are equipped with an en­coder sen­sor (that mea­sures the dis­place­ment or cur­rent po­si­tion of the joint) and the high­er lev­el con­trol sig­nals are the de­sired joint po­si­tions.

Con­trol­ling in joint space

The eas­i­est type of con­trol is to con­trol the ro­bot in what we call the joint-space. This ba­si­cal­ly means that we have some de­sired q^* joint po­si­tions and some de­sired \dot{q}^* joint ve­loc­i­ties (usu­al­ly set to ze­ro) and we want to find the con­trol sig­nal to achieve them.

When con­trol­ling in joint-space, the most clas­si­cal con­troller is a Pro­por­tion­al–In­te­gral–De­riv­a­tive (PID) con­troller. In essence, the de­sired con­trol sig­nal (no mat­ter the type) is com­put­ed as fol­lows:

\tau = K_p (q^*-q) + K_i \int_0^t (q^*-q)dt + K_d\frac{d(q^*-q)}{dt}

The us­er here will need to tune the pa­ram­e­ters K_p,K_i,K_d . In ro­bot­ics, a vari­a­tion of this con­trol is wide­ly used (usu­al­ly named PD-con­troller):

\tau = K_p (q^*-q) + K_d (\dot{q}^*-\dot{q})

Us­ing these sim­ple con­trollers, one can do some very amaz­ing and in­ter­est­ing things.

Con­trol­ling in task space

How­ev­er, most of the in­ter­est­ing things hap­pen in the Carte­sian 6D space. This is the space where the end-ef­fec­tor of our ma­nip­u­la­tor lives in. More­over, this is the space where most of the re­al world tasks can be very in­tu­itive­ly de­scribed. For this rea­son, we call it the task-space.

Ja­co­bian

Ja­co­bian ma­tri­ces are very use­ful, and heav­i­ly used in ro­bot­ics en­gi­neer­ing and re­search. Ba­si­cal­ly, a Ja­co­bian de­fines the dy­nam­ic re­la­tion­ship be­tween two dif­fer­ent rep­re­sen­ta­tions of a sys­tem. For­mal­ly, the Ja­co­bian ma­trix is the first-or­der par­tial de­riv­a­tives of a func­tion with re­spect to some vari­ables. Let’s take as an ex­am­ple our ro­botic ma­nip­u­la­tor. As we have al­ready, we can de­scribe its con­fig­u­ra­tion by the joint po­si­tions q . This con­fig­u­ra­tion al­so gives a 6D po­si­tion (that is, trans­la­tion and ori­en­ta­tion) for the end-ef­fec­tor, let’s call it x(q) . This re­la­tion­ship is known as the for­ward kine­mat­ics and is usu­al­ly non-lin­ear. The Ja­co­bian ma­trix (which is a func­tion), J(q) , gives us the fol­low­ing re­la­tion­ship:

\begin{aligned} J(q) &= \frac{\partial{x(q)}}{\partial{q}}\\ &= \frac{\partial{x(q)}}{\partial{t}}\frac{\partial{t}}{\partial{q}}\Rightarrow\\ \dot{x} &= J(q)\dot{q} \end{aligned}

With this last equa­tion, we have an an­a­lyt­i­cal equa­tion that re­lates the joint ve­loc­i­ties, \dot{q} to end-ef­fec­tor ve­loc­i­ties \dot{x} . More­over, this is a lin­ear re­la­tion­ship. Us­ing the prop­er­ty of con­ser­va­tion of en­er­gy (from tra­di­tion­al physics) and some math­e­mat­i­cal ma­nip­u­la­tion, we can gen­er­al­ize this re­la­tion­ship to forces and torques (we drop the ar­gu­ments in J for clar­i­ty) as

\tau = J^TW

where W is the wrench (force and torque) ap­plied on the end-ef­fec­tor. Us­ing this equa­tion we can plan our de­sired tra­jec­to­ries in the end-ef­fec­tor, write some con­troller in that space (like a PD con­troller that we saw pre­vi­ous­ly), and then trans­form them to joint-space (the space that we can ac­tu­al­ly con­trol).

But how do we get this Ja­co­bian ma­trix? We will not go in­to the de­tails, but once we have the for­ward kine­mat­ics of our ro­bot (this is ei­ther mea­sured or giv­en by the man­u­fac­tur­er), then we just take the par­tial de­riv­a­tives and we get our Ja­co­bian.

Us­ing DART to per­form task-space con­trol

DART has built-in func­tions that com­pute the Ja­co­bian of any link of our ro­bot, and thus con­trol­ling our ro­bots in the task-space is quite easy. Here’s what we do:

/* 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);

See this code in the mag­num-ex­am­ples re­pos­i­to­ry.

For mak­ing the ex­am­ple, we had to do a few more things, like adding a few box­es, keep­ing track of their lo­ca­tion and up­dat­ing the de­sired end-ef­fec­tor lo­ca­tion giv­en the us­er in­put. More­over, we added to our ma­nip­u­la­tor a grip­per so that it can grasp the box­es. In re­al­i­ty, most com­mer­cial­ly avail­able grip­pers are po­si­tion con­trolled (we can­not send torques as we were de­scrib­ing so far, but we need to give de­sired po­si­tions): to sim­u­late that we use ve­loc­i­ty con­trol for the grip­per and what we have been de­scrib­ing so far for the rest of the ma­nip­u­la­tor. For this rea­son, we need to keep a mod­el of our ma­nip­u­la­tor with­out the grip­per so that DART can gives us all the in­for­ma­tion we need clean­ly and eas­i­ly. There is a way to do it with­out a copy, but in­volves keep­ing track of frames and re­mov­ing spe­cif­ic lines from the Ja­co­bian, and thus, I chose the copy to be more clear.

Why DART and not Bul­let?

Bul­let Physics is one of the most pop­u­lar open-source physics en­gines avail­able and wide­ly used in the graph­ics and game in­dus­try. Al­though choos­ing which physics li­brary to use for a project de­pends on quite many pa­ram­e­ters and fac­tors, I am go­ing to list a few rea­sons why I am us­ing DART more ac­tive­ly than Bul­let:

  • DART is ori­ent­ed to­wards com­put­er an­i­ma­tion and ro­bot­ics. This means that it al­ready has im­ple­ment­ed a wide range of func­tion­al­i­ty that is need­ed for these ar­eas. A few ex­am­ples con­sist:
    • For­ward/In­verse kine­mat­ics,
    • For­ward dy­nam­ics,
    • Ja­co­bians and their de­riv­a­tives,
    • Ro­bot dy­nam­ics quan­ti­ties (like mass ma­trix, cori­o­lis and grav­i­ty forces, etc.),
    • Ef­fi­cient im­ple­men­ta­tion for rigid body chains (al­though Bul­let has re­cent­ly added this fea­ture),
    • Mo­tion plan­ning.
  • Mod­ern C++11/14 code,
  • Ac­cu­rate and sta­ble dy­nam­ics for rigid body chains (ro­botic sys­tems).

Al­though Bul­let is mov­ing to­wards sup­port­ing most of the fea­tures that DART al­ready has, to me as a roboti­cist I find DART more in­tu­itive and clos­er to my mind of think­ing. Nev­er­the­less, if some­one is aim­ing for big open worlds (e.g., like in The El­der Scrolls: Skyrim) or many ob­jects in­ter­act­ing with each oth­er, DART is not de­signed for this type of sit­u­a­tions and Bul­let will be sig­nif­i­cant­ly faster (pos­si­bly less ac­cu­rate though).

~ ~ ~

Thank you for read­ing! Be sure to check out the full ex­am­ple code, build it on your ma­chine and play around with its con­trols.

Konstantinos Chatzilygeroudis

About the author

Kon­stanti­nos Chatzi­lyger­oud­is loves to pro­gram ro­bots, C++ and has a hid­den love for com­put­er graph­ics. Post-doc­tor­al fel­low in ro­bot­ics and ma­chine learn­ing at LASA team, EPFL. Fol­low him on Twit­ter: @chatzi­lyge

Guest Posts

Guest Posts

This is a guest post by an ex­ter­nal au­thor. Do you al­so have some­thing in­ter­est­ing to say? A suc­cess sto­ry worth shar­ing? We’ll be hap­py to pub­lish it. See the in­tro­duc­to­ry post for de­tails.