diff --git a/viz/RigidBodyStateVisualization.cpp b/viz/RigidBodyStateVisualization.cpp
index bf082a49..e7b7684c 100644
--- a/viz/RigidBodyStateVisualization.cpp
+++ b/viz/RigidBodyStateVisualization.cpp
@@ -7,13 +7,16 @@
 #include <osg/Material>
 #include <osgFX/BumpMapping>
 #include <osgText/Text>
+#include <QDir>
 
 using namespace osg;
 namespace vizkit3d 
 {
 
+// PUBLIC
 RigidBodyStateVisualization::RigidBodyStateVisualization(QObject* parent)
     : Vizkit3DPlugin<base::samples::RigidBodyState>(parent)
+    , states()
     , covariance(false)
     , covariance_with_samples(false)
     , color(1, 1, 1)
@@ -27,46 +30,316 @@ RigidBodyStateVisualization::RigidBodyStateVisualization(QObject* parent)
     , bump_mapping_dirty(false)
     , forcePositionDisplay(false)
     , forceOrientationDisplay(false)
+    , model_path()
 {
-    state = base::samples::RigidBodyState::invalid();
-    state.position = base::Vector3d::Zero();
-    state.orientation = base::Quaterniond::Identity();
 }
 
 RigidBodyStateVisualization::~RigidBodyStateVisualization()
 {
 }
 
-void RigidBodyStateVisualization::setColor(const Vec4d& color, Geode* geode)
+// PROTECTED
+ref_ptr<Node> RigidBodyStateVisualization::createMainNode()
 {
-    Material *material = new Material();
-    material->setDiffuse(Material::FRONT,  Vec4(0.1, 0.1, 0.1, 1.0));
-    material->setSpecular(Material::FRONT, Vec4(0.6, 0.6, 0.6, 1.0));
-    material->setAmbient(Material::FRONT,  Vec4(0.1, 0.1, 0.1, 1.0));
-    material->setEmission(Material::FRONT, color);
-    material->setShininess(Material::FRONT, 10.0);
+    Group* group = new Group;
 
-    geode->getOrCreateStateSet()->setAttribute(material);    
+    texture = new osg::Texture2D;
+    texture->setWrap(Texture::WRAP_S, Texture::REPEAT);
+    texture->setWrap(Texture::WRAP_T, Texture::REPEAT);
+    texture->setFilter(osg::Texture::MIN_FILTER, osg::Texture::LINEAR_MIPMAP_LINEAR);
+    texture->setFilter(osg::Texture::MAG_FILTER, osg::Texture::LINEAR);
+    diffuse_texture = new osg::Texture2D;
+    diffuse_texture->setWrap(Texture::WRAP_S, Texture::REPEAT);
+    diffuse_texture->setWrap(Texture::WRAP_T, Texture::REPEAT);
+    diffuse_texture->setFilter(osg::Texture::MIN_FILTER, osg::Texture::LINEAR_MIPMAP_LINEAR);
+    diffuse_texture->setFilter(osg::Texture::MAG_FILTER, osg::Texture::LINEAR);
+    normal_texture = new osg::Texture2D;
+    normal_texture->setWrap(Texture::WRAP_S, Texture::REPEAT);
+    normal_texture->setWrap(Texture::WRAP_T, Texture::REPEAT);
+    normal_texture->setFilter(osg::Texture::MIN_FILTER, osg::Texture::LINEAR_MIPMAP_LINEAR);
+    normal_texture->setFilter(osg::Texture::MAG_FILTER, osg::Texture::LINEAR);
+
+    return group;
+}
+
+void RigidBodyStateVisualization::updateMainNode(Node* node)
+{
+    Group* group = node->asGroup();
+    
+    group->removeChildren(0, group->getNumChildren());
+    
+    if (!body_model) {
+        resetModel(total_size);
+    }
+    
+    if (texture_dirty) {
+        updateTexture();
+    }
+    
+    // Bump mapping not added yet, seems not to work anyway.
+    //if (bump_mapping_dirty)
+    //    updateBumpMapping();
+    
+    std::vector<base::samples::RigidBodyState>::iterator it;
+    for(it = states.begin(); it != states.end(); it++) {
+        PositionAttitudeTransform* body_pose = new PositionAttitudeTransform();
+        
+        // Add texture.
+        if(text_size > 0.0 && !it->sourceFrame.empty())
+        {
+            ref_ptr<Geode> geode = new Geode();
+            double actual_size = text_size * total_size;
+            ref_ptr<osgText::Text> text= new osgText::Text;
+            text->setText(it->sourceFrame);
+            text->setCharacterSize(actual_size);
+            text->setPosition(osg::Vec3d(actual_size/2,actual_size/2,0));
+            geode->addDrawable(text);
+            body_pose->addChild(geode);
+        }
+        
+        body_pose->addChild(body_model);
+        group->addChild(body_pose);
+        
+        if (forcePositionDisplay || it->hasValidPosition()) {
+            osg::Vec3d pos(it->position.x(), it->position.y(), it->position.z());
+            body_pose->setPosition(pos + translation);
+        }
+        
+        if (forceOrientationDisplay || it->hasValidOrientation()) {
+            osg::Quat orientation(it->orientation.x(),
+                    it->orientation.y(),
+                    it->orientation.z(),
+                    it->orientation.w());
+            body_pose->setAttitude(rotation * orientation);
+        } 
+        
+        // Add uncertainties if required.
+        bool needs_uncertainty = covariance && it->hasValidPositionCovariance();
+        if(needs_uncertainty) {
+            Uncertainty* uncertainty = new Uncertainty;
+            uncertainty->setMean(static_cast<Eigen::Vector3d>(it->position));
+            uncertainty->setCovariance(static_cast<Eigen::Matrix3d>(it->cov_position));
+            group->addChild(uncertainty);
+            
+            if (covariance_with_samples) {
+                uncertainty->showSamples();
+            } else {
+                uncertainty->hideSamples();
+            }
+        }
+    }
+    
+    /*    
+    // Reset the body model if needed
+    Node* body_node = body_pose->getChild(0);
+    if (bump_mapping && body_node != bump_mapping)
+    {
+        bump_mapping->addChild(body_model);
+        body_pose->setChild(0, bump_mapping);
+    }
+    else if (body_node != body_model)
+        body_pose->setChild(0, body_model);    
+    */
 }
 
+void RigidBodyStateVisualization::updateDataIntern( const base::samples::RigidBodyState& state )
+{
+    states.clear();
+    states.resize(1);
+    states[0] = state;
+}
+
+void RigidBodyStateVisualization::updateDataIntern( const std::vector<base::samples::RigidBodyState>& states )
+{
+    this->states = states; 
+}
+
+// PUBLIC SLOTS
 bool RigidBodyStateVisualization::isPositionDisplayForced() const
 { return forcePositionDisplay; }
+
 void RigidBodyStateVisualization::setPositionDisplayForceFlag(bool flag)
 { forcePositionDisplay = flag; emit propertyChanged("forcePositionDisplay"); }
+
 bool RigidBodyStateVisualization::isOrientationDisplayForced() const
 { return forceOrientationDisplay; }
+
 void RigidBodyStateVisualization::setOrientationDisplayForceFlag(bool flag)
-{ forceOrientationDisplay = flag; emit propertyChanged("forceOrientationDisplay"); }
+{ 
+    forceOrientationDisplay = flag; 
+    emit propertyChanged("forceOrientationDisplay"); 
+}
+
+void RigidBodyStateVisualization::resetModel(double size)
+{
+    body_type  = BODY_SIMPLE;
+    body_model = createSimpleBody(size);
+    setDirty();
+}
+
+void RigidBodyStateVisualization::resetModelSphere(double size)
+{
+    body_type  = BODY_SPHERE;
+    body_model = createSimpleSphere(size);
+    setDirty();
+}
+
+QString RigidBodyStateVisualization::getModelPath() const
+{
+    if (body_type == BODY_SPHERE)
+        return "sphere";
+    else if (body_type == BODY_SIMPLE)
+        return "simple";
+    else
+        return model_path;
+}
+
+void RigidBodyStateVisualization::loadModel(QString const& path)
+{
+    return loadModel(path.toStdString());
+}
+
+void RigidBodyStateVisualization::loadModel(std::string const& path)
+{
+    if (path == "sphere")
+    {
+        resetModelSphere(total_size);
+        return;
+    }
+    else if (path == "simple" || path.empty())
+    {
+        resetModel(total_size);
+        return;
+    }
+
+    QString qt_path = createAbsolutePath(path);
+    
+    // If the model cannot be opened, NULL will be returned.
+    ref_ptr<Node> model = osgDB::readNodeFile(qt_path.toStdString());
+    if(model == NULL) {
+        return;
+    }
+    
+    body_type  = BODY_CUSTOM_MODEL;
+    body_model = model;
+    model_path = qt_path;
+    
+    // Set plugin name.
+    if(vizkit3d_plugin_name.isEmpty())
+    {
+        size_t found;
+        std::string str;
+        found = path.find_last_of("/\\");
+        if(found == std::string::npos)
+            str = path;
+        else
+            str = path.substr(found+1);
+            
+        found = str.find_last_of(".");
+        if(found != std::string::npos)
+        {
+            str = str.substr(0,found);
+            if(!str.empty()) {
+                setPluginName(QString::fromStdString(str));
+            }
+        }
+    }
+
+    setDirty();
+    emit propertyChanged("modelPath");
+}
+
+void RigidBodyStateVisualization::setMainSphereSize(double size)
+{
+    main_size = size;
+    emit propertyChanged("sphereSize");
+    // This triggers an update of the model if we don't have a custom model
+    setSize(total_size);
+}
+
+double RigidBodyStateVisualization::getMainSphereSize() const
+{
+    return main_size;
+}
+
+void RigidBodyStateVisualization::setSize(double size)
+{
+    total_size = size;
+    emit propertyChanged("size");
+    if (body_type == BODY_SIMPLE)
+        resetModel(size);
+    else if (body_type == BODY_SPHERE)
+        resetModelSphere(size);
+}
+
+double RigidBodyStateVisualization::getSize() const
+{
+    return total_size;
+}
+
+void RigidBodyStateVisualization::setTextSize(double size)
+{
+    text_size = size;
+    emit propertyChanged("textSize");
+    // This triggers an update of the model if we don't have a custom model
+    setSize(total_size);
+}
+
+double RigidBodyStateVisualization::getTextSize() const
+{
+    return text_size;
+}
+
+void RigidBodyStateVisualization::displayCovariance(bool enable)
+{ covariance = enable; emit propertyChanged("displayCovariance"); }
+
+bool RigidBodyStateVisualization::isCovarianceDisplayed() const
+{ return covariance; }
+
+void RigidBodyStateVisualization::displayCovarianceWithSamples(bool enable)
+{
+    covariance_with_samples = enable; 
+    emit propertyChanged("displayCovarianceWithSamples"); 
+}
+
+bool RigidBodyStateVisualization::isCovarianceDisplayedWithSamples() const
+{ return covariance_with_samples; }
+
+void RigidBodyStateVisualization::setColor(base::Vector3d const& color)
+{ this->color = color; }
+
+void RigidBodyStateVisualization::setColor(const Vec4d& color, Geode* geode)
+{
+    Material *material = new Material();
+    material->setDiffuse(Material::FRONT,  Vec4(0.1, 0.1, 0.1, 1.0));
+    material->setSpecular(Material::FRONT, Vec4(0.6, 0.6, 0.6, 1.0));
+    material->setAmbient(Material::FRONT,  Vec4(0.1, 0.1, 0.1, 1.0));
+    material->setEmission(Material::FRONT, color);
+    material->setShininess(Material::FRONT, 10.0);
+
+    geode->getOrCreateStateSet()->setAttribute(material);    
+}
 
 void RigidBodyStateVisualization::setTexture(QString const& path)
 { return setTexture(path.toStdString()); }
+
 void RigidBodyStateVisualization::setTexture(std::string const& path)
 {
     if (path.empty())
         return clearTexture();
-
-    image = osgDB::readImageFile(path);
+    
+    QString qt_path = createAbsolutePath(path);
+    
+    image = osgDB::readImageFile(qt_path.toStdString());
+    if(image == NULL) {
+        return;
+    }
     texture_dirty = true;
+    texture_path = path;
+}
+
+QString RigidBodyStateVisualization::getTexture() const {
+    return QString(texture_path.c_str());
 }
 
 void RigidBodyStateVisualization::clearTexture()
@@ -78,15 +351,25 @@ void RigidBodyStateVisualization::clearTexture()
 void RigidBodyStateVisualization::addBumpMapping(
                 QString const& diffuse_color_map_path,
                 QString const& normal_map_path)
-{ return addBumpMapping(diffuse_color_map_path.toStdString(),
-        normal_map_path.toStdString()); }
+{
+    return addBumpMapping(diffuse_color_map_path.toStdString(), 
+            normal_map_path.toStdString()); 
+}
+        
 void RigidBodyStateVisualization::addBumpMapping(
                 std::string const& diffuse_color_map_path,
                 std::string const& normal_map_path)
-{
-    if (!body_model->asGeode())
-    {
-        std::cerr << "model is not a geometry, cannot use bump mapping" << std::endl;
+{   
+    if(body_model == NULL) {
+        return;
+    }
+           
+    if (!body_model->asGeode()) {
+        std::cerr << "model is not a geode, cannot use bump mapping" << std::endl;
+        return;
+    } else if ( !body_model->asGeode()->getDrawable(0) || 
+                !body_model->asGeode()->getDrawable(0)->asGeometry()) {
+        std::cerr << "model does not contain a mesh, cannot use bump mapping" << std::endl;
         return;
     }
 
@@ -103,55 +386,30 @@ void RigidBodyStateVisualization::addBumpMapping(
     bump_mapping_dirty = true;
 }
 
-void RigidBodyStateVisualization::updateTexture()
+void RigidBodyStateVisualization::removeBumpMapping()
 {
-    ref_ptr<StateSet> state = body_model->getOrCreateStateSet();
-    if (!image)
-    {
-        state->setTextureMode(0, GL_TEXTURE_2D, StateAttribute::OFF);
-        return;
-    }
-    else
-    {
-        texture->setImage(image.get());
-        state->setTextureAttributeAndModes(0, texture, StateAttribute::ON);
-    }
+    bump_mapping.release();
+    bump_mapping_dirty = true;
 }
 
-void RigidBodyStateVisualization::updateBumpMapping()
+QVector3D RigidBodyStateVisualization::getTranslation() const
 {
-    ref_ptr<StateSet> state = body_model->getOrCreateStateSet();
-
-    if (!bump_mapping)
-    {
-        diffuse_image.release();
-        normal_image.release();
-        state->setTextureMode(1, GL_TEXTURE_2D, StateAttribute::OFF);
-        state->setTextureMode(2, GL_TEXTURE_2D, StateAttribute::OFF);
-        return;
-    }
-    else
-    {
-
-        ref_ptr<Geometry> geometry = body_model->asGeode()->getDrawable(0)->asGeometry();
-        ref_ptr<Array> tex_coord = geometry->getTexCoordArray(0);
-        geometry->setTexCoordArray(1, tex_coord);
-        geometry->setTexCoordArray(2, tex_coord);
+    return QVector3D(translation.x(), translation.y(), translation.z());
+}
 
-        diffuse_texture->setImage(diffuse_image.get());
-        normal_texture->setImage(normal_image.get());
-        state->setTextureAttributeAndModes(1, normal_texture, StateAttribute::ON);
-        state->setTextureAttributeAndModes(2, diffuse_texture, StateAttribute::ON);
-        bump_mapping->prepareChildren();
-    }
+void RigidBodyStateVisualization::setTranslation(QVector3D const& v)
+{
+    translation = osg::Vec3(v.x(), v.y(), v.z());
+    setDirty();
 }
 
-void RigidBodyStateVisualization::removeBumpMapping()
+void RigidBodyStateVisualization::setRotation(QQuaternion const& q)
 {
-    bump_mapping.release();
-    bump_mapping_dirty = true;
+    rotation = osg::Quat(q.x(), q.y(), q.z(), q.scalar());
+    setDirty();
 }
 
+// PRIVATE
 ref_ptr<Group> RigidBodyStateVisualization::createSimpleSphere(double size)
 {   
     ref_ptr<Group> group = new Group();
@@ -175,16 +433,6 @@ ref_ptr<Group> RigidBodyStateVisualization::createSimpleBody(double size)
     ref_ptr<ShapeDrawable> spd = new ShapeDrawable(sp);
     spd->setColor(Vec4f(color.x(), color.y(), color.z(), 1.0));
     geode->addDrawable(spd);
-    if(text_size>0.0)
-    {
-        double actual_size = text_size * size;
-        ref_ptr<osgText::Text> text= new osgText::Text;
-        text->setText(state.sourceFrame);
-        text->setCharacterSize(actual_size);
-        text->setPosition(osg::Vec3d(actual_size/2,actual_size/2,0));
-        geode->addDrawable(text);
-    }
-
     group->addChild(geode);
     
     //up
@@ -216,248 +464,56 @@ ref_ptr<Group> RigidBodyStateVisualization::createSimpleBody(double size)
     return group;
 }
 
-double RigidBodyStateVisualization::getMainSphereSize() const
-{
-    return main_size;
-}
-
-void RigidBodyStateVisualization::setMainSphereSize(double size)
-{
-    main_size = size;
-    emit propertyChanged("sphereSize");
-    // This triggers an update of the model if we don't have a custom model
-    setSize(total_size);
-}
-
-double RigidBodyStateVisualization::getTextSize() const
-{
-    return text_size;
-}
-
-void RigidBodyStateVisualization::setTextSize(double size)
-{
-    text_size = size;
-    emit propertyChanged("textSize");
-    // This triggers an update of the model if we don't have a custom model
-    setSize(total_size);
-}
-
-void RigidBodyStateVisualization::setSize(double size)
-{
-    total_size = size;
-    emit propertyChanged("size");
-    if (body_type == BODY_SIMPLE)
-        resetModel(size);
-    else if (body_type == BODY_SPHERE)
-        resetModelSphere(size);
-}
-
-double RigidBodyStateVisualization::getSize() const
-{
-    return total_size;
-}
-
-void RigidBodyStateVisualization::resetModel(double size)
-{
-    body_type  = BODY_SIMPLE;
-    body_model = createSimpleBody(size);
-    setDirty();
-}
-
-void RigidBodyStateVisualization::resetModelSphere(double size)
-{
-    body_type  = BODY_SPHERE;
-    body_model = createSimpleSphere(size);
-    setDirty();
-}
-
-QString RigidBodyStateVisualization::getModelPath() const
-{
-    if (body_type == BODY_SPHERE)
-        return "sphere";
-    else if (body_type == BODY_SIMPLE)
-        return "simple";
-    else
-        return model_path;
-}
-
-void RigidBodyStateVisualization::loadModel(QString const& path)
-{
-    return loadModel(path.toStdString());
-}
-
-void RigidBodyStateVisualization::loadModel(std::string const& path)
+void RigidBodyStateVisualization::updateTexture()
 {
-    if (path == "sphere")
-    {
-        resetModelSphere(total_size);
-        return;
-    }
-    else if (path == "simple")
+    ref_ptr<StateSet> state = body_model->getOrCreateStateSet();
+    if (!image)
     {
-        resetModel(total_size);
+        state->setTextureMode(0, GL_TEXTURE_2D, StateAttribute::OFF);
         return;
     }
-
-    ref_ptr<Node> model = osgDB::readNodeFile(path);
-    body_type  = BODY_CUSTOM_MODEL;
-    body_model = model;
-    if (!body_model->asGeode())
-        std::cerr << "model is not a geode, using bump mapping will not be possible" << std::endl;
-    else if (!body_model->asGeode()->getDrawable(0))
-        std::cerr << "model does not contain a mesh, using bump mapping will not be possible" << std::endl;
-    else if (!body_model->asGeode()->getDrawable(0)->asGeometry())
-        std::cerr << "model does not contain a mesh, using bump mapping will not be possible" << std::endl;
-
-    model_path = QString::fromStdString(path);
-    //set plugin name
-    if(vizkit3d_plugin_name.isEmpty())
+    else
     {
-        size_t found;
-        std::string str;
-        found = path.find_last_of("/\\");
-        if(found == std::string::npos)
-            str = path;
-        else
-            str = path.substr(found+1);
-        found = str.find_last_of(".");
-        if(found != std::string::npos)
-        {
-            str = str.substr(0,found);
-            if(!str.empty())
-                setPluginName(QString::fromStdString(str));
-        }
+        texture->setImage(image.get());
+        state->setTextureAttributeAndModes(0, texture, StateAttribute::ON);
     }
-
-    setDirty();
-    emit propertyChanged("modelPath");
-}
-QVector3D RigidBodyStateVisualization::getTranslation() const
-{
-    return QVector3D(translation.x(), translation.y(), translation.z());
-}
-void RigidBodyStateVisualization::setTranslation(QVector3D const& v)
-{
-    translation = osg::Vec3(v.x(), v.y(), v.z());
-    setDirty();
 }
 
-void RigidBodyStateVisualization::setRotation(QQuaternion const& q)
-{
-    rotation = osg::Quat(q.x(), q.y(), q.z(), q.scalar());
-    setDirty();
-}
-
-void RigidBodyStateVisualization::displayCovariance(bool enable)
-{ covariance = enable; emit propertyChanged("displayCovariance"); }
-bool RigidBodyStateVisualization::isCovarianceDisplayed() const
-{ return covariance; }
-
-void RigidBodyStateVisualization::setColor(base::Vector3d const& color)
-{ this->color = color; }
-
-void RigidBodyStateVisualization::displayCovarianceWithSamples(bool enable)
-{ covariance_with_samples = enable; emit propertyChanged("displayCovarianceWithSamples"); }
-bool RigidBodyStateVisualization::isCovarianceDisplayedWithSamples() const
-{ return covariance_with_samples; }
-
-ref_ptr<Node> RigidBodyStateVisualization::createMainNode()
-{
-    Group* group = new Group;
-    PositionAttitudeTransform* body_pose =
-        new PositionAttitudeTransform();
-    if (!body_model)
-        resetModel(total_size);
-    body_pose->addChild(body_model);
-    group->addChild(body_pose);
-
-    texture = new osg::Texture2D;
-    texture->setWrap(Texture::WRAP_S, Texture::REPEAT);
-    texture->setWrap(Texture::WRAP_T, Texture::REPEAT);
-    texture->setFilter(osg::Texture::MIN_FILTER, osg::Texture::LINEAR_MIPMAP_LINEAR);
-    texture->setFilter(osg::Texture::MAG_FILTER, osg::Texture::LINEAR);
-    diffuse_texture = new osg::Texture2D;
-    diffuse_texture->setWrap(Texture::WRAP_S, Texture::REPEAT);
-    diffuse_texture->setWrap(Texture::WRAP_T, Texture::REPEAT);
-    diffuse_texture->setFilter(osg::Texture::MIN_FILTER, osg::Texture::LINEAR_MIPMAP_LINEAR);
-    diffuse_texture->setFilter(osg::Texture::MAG_FILTER, osg::Texture::LINEAR);
-    normal_texture = new osg::Texture2D;
-    normal_texture->setWrap(Texture::WRAP_S, Texture::REPEAT);
-    normal_texture->setWrap(Texture::WRAP_T, Texture::REPEAT);
-    normal_texture->setFilter(osg::Texture::MIN_FILTER, osg::Texture::LINEAR_MIPMAP_LINEAR);
-    normal_texture->setFilter(osg::Texture::MAG_FILTER, osg::Texture::LINEAR);
-
-    return group;
-}
-
-void RigidBodyStateVisualization::updateMainNode(Node* node)
+void RigidBodyStateVisualization::updateBumpMapping()
 {
-    Group* group = node->asGroup();
-    PositionAttitudeTransform* body_pose =
-        dynamic_cast<PositionAttitudeTransform*>(group->getChild(0));
-
-    // Check if we need an uncertainty representation node, and manage the
-    // uncertainty child accordingly
-    bool needs_uncertainty = covariance && state.hasValidPositionCovariance();
-    Uncertainty* uncertainty = 0;
-    if (group->getNumChildren() > 1)
-    {
-        if (needs_uncertainty)
-            uncertainty = dynamic_cast<Uncertainty*>(group->getChild(1));
-        else
-            group->removeChild(1);
-    }
-    else if (needs_uncertainty)
-    {
-        uncertainty = new Uncertainty;
-        group->addChild(uncertainty);
-    }
-
-    // Reset the body model if needed
-    Node* body_node = body_pose->getChild(0);
-    if (bump_mapping && body_node != bump_mapping)
-    {
-        bump_mapping->addChild(body_model);
-        body_pose->setChild(0, bump_mapping);
-    }
-    else if (body_node != body_model)
-        body_pose->setChild(0, body_model);
-
-    if (texture_dirty)
-        updateTexture();
-    if (bump_mapping_dirty)
-        updateBumpMapping();
+    ref_ptr<StateSet> state = body_model->getOrCreateStateSet();
 
-    if (forcePositionDisplay || state.hasValidPosition())
+    if (!bump_mapping)
     {
-	osg::Vec3d pos(
-                state.position.x(), state.position.y(), state.position.z());
-        
-        body_pose->setPosition(pos + translation);
+        diffuse_image.release();
+        normal_image.release();
+        state->setTextureMode(1, GL_TEXTURE_2D, StateAttribute::OFF);
+        state->setTextureMode(2, GL_TEXTURE_2D, StateAttribute::OFF);
+        return;
     }
-    if (needs_uncertainty)
+    else
     {
-        if (covariance_with_samples)
-            uncertainty->showSamples();
-        else
-            uncertainty->hideSamples();
+        ref_ptr<Geometry> geometry = body_model->asGeode()->getDrawable(0)->asGeometry();
+        ref_ptr<Array> tex_coord = geometry->getTexCoordArray(0);
+        geometry->setTexCoordArray(1, tex_coord);
+        geometry->setTexCoordArray(2, tex_coord);
 
-        uncertainty->setMean(static_cast<Eigen::Vector3d>(state.position));
-        uncertainty->setCovariance(static_cast<Eigen::Matrix3d>(state.cov_position));
-    }
-    if (forceOrientationDisplay || state.hasValidOrientation())
-    {
-	osg::Quat orientation(state.orientation.x(),
-                state.orientation.y(),
-                state.orientation.z(),
-                state.orientation.w());
-        body_pose->setAttitude(rotation * orientation);
+        diffuse_texture->setImage(diffuse_image.get());
+        normal_texture->setImage(normal_image.get());
+        state->setTextureAttributeAndModes(1, normal_texture, StateAttribute::ON);
+        state->setTextureAttributeAndModes(2, diffuse_texture, StateAttribute::ON);
+        bump_mapping->prepareChildren();
     }
 }
 
-void RigidBodyStateVisualization::updateDataIntern( const base::samples::RigidBodyState& state )
-{
-    this->state = state;
+QString RigidBodyStateVisualization::createAbsolutePath(std::string const& path){
+    QString qt_path(path.c_str());
+    if (qt_path.startsWith ("~/")) {
+        qt_path.replace (0, 1, QDir::homePath());
+    }   
+    QDir dir(qt_path);
+    qt_path = dir.absolutePath();
+    return qt_path;
 }
 
-}
+} // end namespace vizkit3d 
\ No newline at end of file
diff --git a/viz/RigidBodyStateVisualization.hpp b/viz/RigidBodyStateVisualization.hpp
index 11da90b0..69f6c2e7 100644
--- a/viz/RigidBodyStateVisualization.hpp
+++ b/viz/RigidBodyStateVisualization.hpp
@@ -16,7 +16,8 @@ namespace osgFX
 namespace vizkit3d 
 {
 
-class RigidBodyStateVisualization : public Vizkit3DPlugin<base::samples::RigidBodyState>
+class RigidBodyStateVisualization : public Vizkit3DPlugin<base::samples::RigidBodyState>,
+        public VizPluginAddType<std::vector <base::samples::RigidBodyState> >
 {
         Q_OBJECT
         Q_PROPERTY(double size READ getSize WRITE setSize)
@@ -27,22 +28,26 @@ class RigidBodyStateVisualization : public Vizkit3DPlugin<base::samples::RigidBo
         Q_PROPERTY(bool forcePositionDisplay READ isPositionDisplayForced WRITE setPositionDisplayForceFlag)
         Q_PROPERTY(bool forceOrientationDisplay READ isOrientationDisplayForced WRITE setOrientationDisplayForceFlag)
         Q_PROPERTY(QString modelPath READ getModelPath WRITE loadModel)
+        Q_PROPERTY(QString texturePath READ getTexture WRITE setTexture)
 
     public:
-	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-	RigidBodyStateVisualization(QObject* parent = NULL);
-	virtual ~RigidBodyStateVisualization();
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+        RigidBodyStateVisualization(QObject* parent = NULL);
+        virtual ~RigidBodyStateVisualization();
 
         Q_INVOKABLE void updateData( const base::samples::RigidBodyState& state )
         { return Vizkit3DPlugin<base::samples::RigidBodyState>::updateData(state); }
         Q_INVOKABLE void updateRigidBodyState( const base::samples::RigidBodyState& state )
         { return updateData(state); }
+        Q_INVOKABLE void updateData( const std::vector<base::samples::RigidBodyState>& states )
+        { return Vizkit3DPlugin<base::samples::RigidBodyState>::updateData(states); }
 
     protected:
         virtual osg::ref_ptr<osg::Node> createMainNode();
-	virtual void updateMainNode(osg::Node* node);
-	void updateDataIntern( const base::samples::RigidBodyState& state );
-        base::samples::RigidBodyState state;
+        virtual void updateMainNode(osg::Node* node);
+        void updateDataIntern( const base::samples::RigidBodyState& state );
+        void updateDataIntern( const std::vector<base::samples::RigidBodyState>& states );
+        std::vector<base::samples::RigidBodyState> states;
     
     public slots: 
         bool isPositionDisplayForced() const;
@@ -50,11 +55,8 @@ class RigidBodyStateVisualization : public Vizkit3DPlugin<base::samples::RigidBo
         bool isOrientationDisplayForced() const;
         void setOrientationDisplayForceFlag(bool flag);
 
-        double getSize() const;
-        void setSize(double size);
-
         void resetModel(double size);
-	void resetModelSphere(double size);
+        void resetModelSphere(double size);
 	
         QString getModelPath() const;
         void loadModel(std::string const& path);
@@ -73,6 +75,9 @@ class RigidBodyStateVisualization : public Vizkit3DPlugin<base::samples::RigidBo
          * The default is 0.1
          */
         double getMainSphereSize() const;
+        
+        double getSize() const;
+        void setSize(double size);
 
         /** Sets the text size relative to the size of the complete object.
          * If text size is positive, the name of the source frame is rendered in the visualization.
@@ -94,12 +99,13 @@ class RigidBodyStateVisualization : public Vizkit3DPlugin<base::samples::RigidBo
          * apply the new color
          */
         void setColor(base::Vector3d const& color);
-	
-	void setColor(const osg::Vec4d& color, osg::Geode* geode);
+        void setColor(const osg::Vec4d& color, osg::Geode* geode);
 	
         void setTexture(QString const& path);
         void setTexture(std::string const& path);
+        QString getTexture() const;     
         void clearTexture();
+        
         void addBumpMapping(
                 QString const& diffuse_color_map_path,
                 QString const& normal_map_path);
@@ -127,14 +133,11 @@ class RigidBodyStateVisualization : public Vizkit3DPlugin<base::samples::RigidBo
         { BODY_NONE, BODY_SIMPLE, BODY_SPHERE, BODY_CUSTOM_MODEL };
 
         BODY_TYPES body_type;
-	osg::ref_ptr<osg::Node>  body_model;
-        osg::ref_ptr<osg::Group> createSimpleBody(double size);
-	osg::ref_ptr<osg::Group> createSimpleSphere(double size);
+        osg::ref_ptr<osg::Node>  body_model;
 
         osg::ref_ptr<osg::Image> image;
         osg::ref_ptr<osg::Texture2D> texture;
         bool texture_dirty;
-        void updateTexture();
 
         osg::ref_ptr<osg::Image> diffuse_image;
         osg::ref_ptr<osg::Image> normal_image;
@@ -142,13 +145,18 @@ class RigidBodyStateVisualization : public Vizkit3DPlugin<base::samples::RigidBo
         osg::ref_ptr<osg::Texture2D> normal_texture;
         osg::ref_ptr<osgFX::BumpMapping> bump_mapping;
         bool bump_mapping_dirty;
-        void updateBumpMapping();
 
         bool forcePositionDisplay;
         bool forceOrientationDisplay;
         
         QString model_path;
-
+        std::string texture_path;
+        
+        osg::ref_ptr<osg::Group> createSimpleBody(double size);
+        osg::ref_ptr<osg::Group> createSimpleSphere(double size);
+        void updateTexture();
+        void updateBumpMapping();
+        QString createAbsolutePath(std::string const& path);
 };
 
 }
diff --git a/viz/vizkit_plugin.rb b/viz/vizkit_plugin.rb
index 5317dd31..3387aaac 100644
--- a/viz/vizkit_plugin.rb
+++ b/viz/vizkit_plugin.rb
@@ -11,6 +11,7 @@
 Vizkit::UiLoader.register_3d_plugin_for('RigidBodyStateVisualization', "/base/samples/RigidBodyState", :updateRigidBodyState)
 Vizkit::UiLoader.register_3d_plugin('BodyStateVisualization',"base", 'BodyStateVisualization')
 Vizkit::UiLoader.register_3d_plugin_for('BodyStateVisualization', "/base/samples/BodyState", :updateBodyState)
+Vizkit::UiLoader.register_3d_plugin_for('RigidBodyStateVisualization', "/std/vector</base/samples/RigidBodyState>", :updateData)
 Vizkit::UiLoader.register_3d_plugin('LaserScanVisualization',"base", 'LaserScanVisualization')
 Vizkit::UiLoader.register_3d_plugin_for('LaserScanVisualization', "/base/samples/LaserScan", :updateLaserScan)
 Vizkit::UiLoader.register_3d_plugin('MotionCommandVisualization',"base", 'MotionCommandVisualization')