diff --git a/src/rviz/robot/robot_link.cpp b/src/rviz/robot/robot_link.cpp
index 4d9a4ca6e9..8b5f6a9b8a 100644
--- a/src/rviz/robot/robot_link.cpp
+++ b/src/rviz/robot/robot_link.cpp
@@ -441,7 +441,7 @@ void RobotLink::updateVisibility()
   }
 }
 
-Ogre::MaterialPtr RobotLink::getMaterialForLink( const urdf::LinkConstSharedPtr& link)
+Ogre::MaterialPtr RobotLink::getMaterialForLink( const urdf::LinkConstSharedPtr& link, const std::string material_name)
 {
   if (!link->visual || !link->visual->material)
   {
@@ -454,9 +454,23 @@ Ogre::MaterialPtr RobotLink::getMaterialForLink( const urdf::LinkConstSharedPtr&
 
   Ogre::MaterialPtr mat = Ogre::MaterialManager::getSingleton().create(ss.str(), ROS_PACKAGE_NAME);
   mat->getTechnique(0)->setLightingEnabled(true);
-  if (link->visual->material->texture_filename.empty())
+
+  boost::shared_ptr<urdf::Visual> visual = link->visual;
+  std::vector<urdf::VisualSharedPtr >::const_iterator vi;
+  for( vi = link->visual_array.begin(); vi != link->visual_array.end(); vi++ )
   {
-    const urdf::Color& col = link->visual->material->color;
+    if( (*vi) && material_name != "" && (*vi)->material_name  == material_name) {
+      visual = *vi;
+      break;
+    }
+  }
+  if ( vi == link->visual_array.end() ) {
+    visual = link->visual; // if link does not have material, use default oneee
+  }
+
+  if (visual->material->texture_filename.empty())
+  {
+    const urdf::Color& col = visual->material->color;
     mat->getTechnique(0)->setAmbient(col.r * 0.5, col.g * 0.5, col.b * 0.5);
     mat->getTechnique(0)->setDiffuse(col.r, col.g, col.b, col.a);
 
@@ -464,7 +478,7 @@ Ogre::MaterialPtr RobotLink::getMaterialForLink( const urdf::LinkConstSharedPtr&
   }
   else
   {
-    std::string filename = link->visual->material->texture_filename;
+    std::string filename = visual->material->texture_filename;
     if (!Ogre::TextureManager::getSingleton().resourceExists(filename))
     {
       resource_retriever::Retriever retriever;
@@ -509,7 +523,7 @@ Ogre::MaterialPtr RobotLink::getMaterialForLink( const urdf::LinkConstSharedPtr&
   return mat;
 }
 
-void RobotLink::createEntityForGeometryElement(const urdf::LinkConstSharedPtr& link, const urdf::Geometry& geom, const urdf::Pose& origin, Ogre::SceneNode* scene_node, Ogre::Entity*& entity)
+void RobotLink::createEntityForGeometryElement(const urdf::LinkConstSharedPtr& link, const urdf::Geometry& geom, const urdf::Pose& origin, const std::string material_name, Ogre::SceneNode* scene_node, Ogre::Entity*& entity)
 {
   entity = NULL; // default in case nothing works.
   Ogre::SceneNode* offset_node = scene_node->createChildSceneNode();
@@ -621,7 +635,17 @@ void RobotLink::createEntityForGeometryElement(const urdf::LinkConstSharedPtr& l
 
     for (uint32_t i = 0; i < entity->getNumSubEntities(); ++i)
     {
+      default_material_ = getMaterialForLink(link, material_name);
+      static int count = 0;
+      std::stringstream ss;
+      ss << default_material_->getName() << count++ << "Robot";
+      std::string cloned_name = ss.str();
+
+      default_material_ = default_material_->clone(cloned_name);
+      default_material_name_ = default_material_->getName();
+
       // Assign materials only if the submesh does not have one already
+
       Ogre::SubEntity* sub = entity->getSubEntity(i);
       const std::string& material_name = sub->getMaterialName();
 
@@ -680,7 +704,7 @@ void RobotLink::createCollision(const urdf::LinkConstSharedPtr& link)
     if( collision && collision->geometry )
     {
       Ogre::Entity* collision_mesh = NULL;
-      createEntityForGeometryElement( link, *collision->geometry, collision->origin, collision_node_, collision_mesh );
+      createEntityForGeometryElement( link, *collision->geometry, collision->origin, "", collision_node_, collision_mesh );
       if( collision_mesh )
       {
         collision_meshes_.push_back( collision_mesh );
@@ -693,7 +717,7 @@ void RobotLink::createCollision(const urdf::LinkConstSharedPtr& link)
   if( !valid_collision_found && link->collision && link->collision->geometry )
   {
     Ogre::Entity* collision_mesh = NULL;
-    createEntityForGeometryElement( link, *link->collision->geometry, link->collision->origin, collision_node_, collision_mesh );
+    createEntityForGeometryElement( link, *link->collision->geometry, link->collision->origin, "", collision_node_, collision_mesh );
     if( collision_mesh )
     {
       collision_meshes_.push_back( collision_mesh );
@@ -737,7 +761,7 @@ void RobotLink::createVisual(const urdf::LinkConstSharedPtr& link )
     if( visual && visual->geometry )
     {
       Ogre::Entity* visual_mesh = NULL;
-      createEntityForGeometryElement( link, *visual->geometry, visual->origin, visual_node_, visual_mesh );
+      createEntityForGeometryElement( link, *visual->geometry, visual->origin, visual->material_name, visual_node_, visual_mesh );
       if( visual_mesh )
       {
         visual_meshes_.push_back( visual_mesh );
@@ -750,7 +774,7 @@ void RobotLink::createVisual(const urdf::LinkConstSharedPtr& link )
   if( !valid_visual_found && link->visual && link->visual->geometry )
   {
     Ogre::Entity* visual_mesh = NULL;
-    createEntityForGeometryElement( link, *link->visual->geometry, link->visual->origin, visual_node_, visual_mesh );
+    createEntityForGeometryElement( link, *link->visual->geometry, link->visual->origin, link->visual->material_name, visual_node_, visual_mesh );
     if( visual_mesh )
     {
       visual_meshes_.push_back( visual_mesh );
diff --git a/src/rviz/robot/robot_link.h b/src/rviz/robot/robot_link.h
index f20cbe34cc..c9217d04e6 100644
--- a/src/rviz/robot/robot_link.h
+++ b/src/rviz/robot/robot_link.h
@@ -155,12 +155,12 @@ private Q_SLOTS:
 private:
   void setRenderQueueGroup( Ogre::uint8 group );
   bool getEnabled() const;
-  void createEntityForGeometryElement( const urdf::LinkConstSharedPtr& link, const urdf::Geometry& geom, const urdf::Pose& origin, Ogre::SceneNode* scene_node, Ogre::Entity*& entity );
+  void createEntityForGeometryElement( const urdf::LinkConstSharedPtr& link, const urdf::Geometry& geom, const urdf::Pose& origin, const std::string material_name, Ogre::SceneNode* scene_node, Ogre::Entity*& entity );
 
   void createVisual( const urdf::LinkConstSharedPtr& link);
   void createCollision( const urdf::LinkConstSharedPtr& link);
   void createSelection();
-  Ogre::MaterialPtr getMaterialForLink( const urdf::LinkConstSharedPtr& link );
+  Ogre::MaterialPtr getMaterialForLink( const urdf::LinkConstSharedPtr& link, const std::string material_name = "" );
 
 
 protected: