Commits

Jose Luis Rivero committed f945b4b Merge

Merge from default

  • Participants
  • Parent commits 55b02b9, 7df9b68
  • Branches classify-tests

Comments (0)

Files changed (159)

 883b81dd41736c40e3cebc7e86b023370057b973 gazebo_1.5.0
 883b81dd41736c40e3cebc7e86b023370057b973 gazebo_1.5.0
 fa08af4f1e7a315818dc9a8f13c28d140d23a638 gazebo_1.5.0
+8f39a7a1ac59453847f5e4eb3da573b3711e9573 gazebo-prerelease_1.6.0
+ca300de8db0cbaf7606212eed0d608c94d96f8d5 gazebo_1.6.0
+b8c85da587931ea2c4d92e1e176f19336eff2f4e gazebo_1.6.1
+b8c85da587931ea2c4d92e1e176f19336eff2f4e gazebo_1.6.1
+16411655b125f2f7bca74043f46ab58590d5c68c gazebo_1.6.1

File CMakeLists.txt

 # The patch version may have been bumped for prerelease purposes; be sure to
 # check gazebo-release/ubuntu/debian/changelog@default to determine what the
 # next patch version should be for a regular release.
-set (GAZEBO_PATCH_VERSION 0)
+set (GAZEBO_PATCH_VERSION 1)
 
 set (GAZEBO_VERSION ${GAZEBO_MAJOR_VERSION}.${GAZEBO_MINOR_VERSION})
 set (GAZEBO_VERSION_FULL ${GAZEBO_MAJOR_VERSION}.${GAZEBO_MINOR_VERSION}.${GAZEBO_PATCH_VERSION})
 MESSAGE(STATUS "Checking gazebo build type")
 # Set the default build type
 if (NOT CMAKE_BUILD_TYPE)
-    set (CMAKE_BUILD_TYPE "RelWithDebInfo" CACHE STRING 
+    set (CMAKE_BUILD_TYPE "Release" CACHE STRING 
         "Choose the type of build, options are: Debug Release RelWithDebInfo Profile Check" FORCE)
 endif (NOT CMAKE_BUILD_TYPE)
 # TODO: still convert to uppercase to keep backwards compatibility with 

File cmake/CheckDRIDisplay.cmake

 MESSAGE(STATUS "Looking for display capabilities")
 SET (VALID_DISPLAY FALSE)
 SET (VALID_DRI_DISPLAY FALSE)
+SET (CHECKER_ERROR "(no glxinfo or pyopengl)")
 
 IF((DEFINED ENV{DISPLAY}) AND NOT ("$ENV{DISPLAY}" STREQUAL ""))
   MESSAGE(STATUS " + found a display available ($DISPLAY is set)")
     IF (GLX)
       MESSAGE(STATUS " + found a valid dri display (glxinfo)")
       SET (VALID_DRI_DISPLAY TRUE)
+    ELSE()
+      SET (CHECKER_ERROR "using glxinfo")
     ENDIF ()
   ELSE ()
     EXECUTE_PROCESS(
       # returns 0 if ok and 1 if error (inverse than cmake IF)
       COMMAND ${PROJECT_SOURCE_DIR}/tools/gl-test.py
       RESULT_VARIABLE GL_FAIL_RESULT
-      ERROR_QUIET
+      ERROR_VARIABLE GL_ERROR
       OUTPUT_QUIET)
 
     IF (NOT GL_FAIL_RESULT)
       MESSAGE(STATUS " + found a valid dri display (pyopengl)")
       SET (VALID_DRI_DISPLAY TRUE)
+   ELSE()
+      # Check error string: no python module means no pyopengl
+      STRING(FIND ${GL_ERROR} 
+              "ImportError: No module named OpenGL.GLUT" ERROR_POS)
+      # -1 will imply pyopengl is present but real DRI test fails
+      IF ("${ERROR_POS}" STREQUAL "-1")
+        SET (CHECKER_ERROR "using pyopengl")
+      ENDIF ()
     ENDIF ()
   ENDIF ()
 ENDIF ()
 ENDIF ()
 
 IF (NOT VALID_DRI_DISPLAY)
-  MESSAGE(STATUS " ! valid dri display not found")
+    MESSAGE(STATUS " ! valid dri display not found ${CHECKER_ERROR}")
 ENDIF ()

File cmake/GazeboUtils.cmake

    foreach(QTEST_SOURCE_file ${ARGN})
      string(REGEX REPLACE ".cc" "" BINARY_NAME ${QTEST_SOURCE_file})
      string(REGEX REPLACE ".cc" ".hh" QTEST_HEADER_file ${QTEST_SOURCE_file})
-     QT4_WRAP_CPP(${BINARY_NAME}_MOC ${QTEST_HEADER_file} QTestFixture.hh)
+     QT4_WRAP_CPP(${BINARY_NAME}_MOC ${QTEST_HEADER_file} ${CMAKE_SOURCE_DIR}/gazebo/gui/QTestFixture.hh)
 
      add_executable(${BINARY_NAME}
-      ${${BINARY_NAME}_MOC} ${QTEST_SOURCE_file} QTestFixture.cc)
+      ${${BINARY_NAME}_MOC} ${QTEST_SOURCE_file} ${CMAKE_SOURCE_DIR}/gazebo/gui/QTestFixture.cc)
 
     add_dependencies(${BINARY_NAME}
       gazebo_gui
       ${QT_LIBRARIES}
       )
 
-    add_test(${BINARY_NAME} ${CMAKE_CURRENT_BINARY_DIR}/${TEST_TYPE}_${BINARY_NAME} -xml)
+    # QTest need and extra -o parameter to write logging information to a file
+    add_test(${BINARY_NAME} ${CMAKE_CURRENT_BINARY_DIR}/${BINARY_NAME}
+	-xml -o ${CMAKE_BINARY_DIR}/test_results/${TEST_TYPE}_${BINARY_NAME}.xml)
+
 
     set_tests_properties(${BINARY_NAME} PROPERTIES TIMEOUT 240)
 

File deps/opende/src/joints/contact.cpp

             t1[1] = contact.fdir1[1];
             t1[2] = contact.fdir1[2];
             dCalcVectorCross3( t2, normal, t1 );
+
+            // if fdir1 is parallel to normal, revert to dPlaneSpace
+            if (_dequal(t2[0], 0.0) &&
+                _dequal(t2[1], 0.0) &&
+                _dequal(t2[2], 0.0))
+              dPlaneSpace( normal, t1, t2 );
         }
         else
         {

File examples/plugins/camera/camera_move.cc

 
       // Listen to the update event. This event is broadcast every
       // simulation iteration.
-      this->updateConnection = event::Events::ConnectWorldUpdateStart(
+      this->updateConnection = event::Events::ConnectWorldUpdateBegin(
           boost::bind(&CameraMove::OnUpdate, this));
     }
 

File examples/plugins/custom_messages/custom_messages.cc

 
       // Listen to the update event. This event is broadcast every
       // simulation iteration.
-      this->updateConnection = event::Events::ConnectWorldUpdateStart(
+      this->updateConnection = event::Events::ConnectWorldUpdateBegin(
           boost::bind(&CustomMessages::OnUpdate, this));
     }
 

File examples/plugins/model_push/model_push.cc

 
       // Listen to the update event. This event is broadcast every
       // simulation iteration.
-      this->updateConnection = event::Events::ConnectWorldUpdateStart(
+      this->updateConnection = event::Events::ConnectWorldUpdateBegin(
           boost::bind(&ModelPush::OnUpdate, this));
     }
 

File examples/plugins/pr2_pose_test.cc

 
       // Listen to the update event. This event is broadcast every
       // simulation iteration.
-      this->updateConnection = event::Events::ConnectWorldUpdateStart(
+      this->updateConnection = event::Events::ConnectWorldUpdateBegin(
           boost::bind(&PR2PoseTest::OnUpdate, this));
       gzdbg << "plugin model name: " << modelName << "\n";
 

File examples/plugins/projector/projector.cc

 
       // Listen to the update event. This event is broadcast every
       // simulation iteration.
-      this->updateConnection = event::Events::ConnectWorldUpdateStart(
+      this->updateConnection = event::Events::ConnectWorldUpdateBegin(
           boost::bind(&ProjectorPlugin::OnUpdate, this));
     }
 

File examples/plugins/ray_test.cc

 
       // Listen to the update event. This event is broadcast every
       // simulation iteration.
-      this->updateConnection = event::Events::ConnectWorldUpdateStart(
+      this->updateConnection = event::Events::ConnectWorldUpdateBegin(
           boost::bind(&RayTest::OnUpdate, this));
       gzdbg << "plugin model name: " << modelName << "\n";
 

File gazebo/Master.cc

 {
   this->stop = false;
   this->runThread = NULL;
-
-  this->connectionMutex = new boost::recursive_mutex();
-  this->msgsMutex = new boost::recursive_mutex();
 }
 
 /////////////////////////////////////////////////
 Master::~Master()
 {
   this->Fini();
-
-  delete this->connectionMutex;
-  this->connectionMutex = NULL;
-
-  delete this->msgsMutex;
-  this->msgsMutex = NULL;
-
-  delete this->runThread;
-  this->runThread = NULL;
-
-  this->publishers.clear();
-  this->subscribers.clear();
-  this->connections.clear();
-
-  this->connection->Shutdown();
-  delete this->connection;
-  this->connection = NULL;
 }
 
 /////////////////////////////////////////////////
 
   // Add the connection to our list
   {
-    boost::recursive_mutex::scoped_lock lock(*this->connectionMutex);
+    boost::recursive_mutex::scoped_lock lock(this->connectionMutex);
     int index = this->connections.size();
 
     this->connections[index] = _newConnection;
   // Store the message if it's not empty
   if (!_data.empty())
   {
-    boost::recursive_mutex::scoped_lock lock(*this->msgsMutex);
+    boost::recursive_mutex::scoped_lock lock(this->msgsMutex);
     this->msgs.push_back(std::make_pair(_connectionIndex, _data));
   }
   else
         worldNameMsg.data());
     if (iter == this->worldNames.end())
     {
-      boost::recursive_mutex::scoped_lock lock(*this->connectionMutex);
+      boost::recursive_mutex::scoped_lock lock(this->connectionMutex);
       this->worldNames.push_back(worldNameMsg.data());
 
       Connection_M::iterator iter2;
   }
   else if (packet.type() == "advertise")
   {
-    boost::recursive_mutex::scoped_lock lock(*this->connectionMutex);
+    boost::recursive_mutex::scoped_lock lock(this->connectionMutex);
     msgs::Publish pub;
     pub.ParseFromString(packet.serialized_data());
 
 
   // Process the incoming message queue
   {
-    boost::recursive_mutex::scoped_lock lock(*this->msgsMutex);
+    boost::recursive_mutex::scoped_lock lock(this->msgsMutex);
     while (this->msgs.size() > 0)
     {
       this->ProcessMessage(this->msgs.front().first,
 
   // Process all the connections
   {
-    boost::recursive_mutex::scoped_lock lock(*this->connectionMutex);
+    boost::recursive_mutex::scoped_lock lock(this->connectionMutex);
     for (iter = this->connections.begin();
         iter != this->connections.end();)
     {
 
   // Remove all messages for this connection
   {
-    boost::recursive_mutex::scoped_lock lock(*this->msgsMutex);
+    boost::recursive_mutex::scoped_lock lock(this->msgsMutex);
     msgIter = this->msgs.begin();
     while (msgIter != this->msgs.end())
     {
 void Master::RemovePublisher(const msgs::Publish _pub)
 {
   {
-    boost::recursive_mutex::scoped_lock lock(*this->connectionMutex);
+    boost::recursive_mutex::scoped_lock lock(this->connectionMutex);
     Connection_M::iterator iter2;
     for (iter2 = this->connections.begin();
         iter2 != this->connections.end(); ++iter2)
 void Master::Fini()
 {
   this->Stop();
+
+  if (this->connection)
+    this->connection->Shutdown();
+  this->connection.reset();
+
+  delete this->runThread;
+  this->runThread = NULL;
+
+  this->msgs.clear();
+  this->worldNames.clear();
+  this->connections.clear();
+  this->subscribers.clear();
+  this->publishers.clear();
 }
 
 //////////////////////////////////////////////////
   Connection_M::iterator iter;
 
   {
-    boost::recursive_mutex::scoped_lock lock(*this->connectionMutex);
+    boost::recursive_mutex::scoped_lock lock(this->connectionMutex);
     for (iter = this->connections.begin();
         iter != this->connections.end(); ++iter)
     {

File gazebo/Master.hh

     private: std::list<std::pair<unsigned int, std::string> > msgs;
 
     /// \brief Our server connection.
-    private: transport::Connection *connection;
+    private: transport::ConnectionPtr connection;
 
     /// \brief Thread to run the main loop.
     private: boost::thread *runThread;
     private: bool stop;
 
     /// \brief Mutex to protect connections.
-    private: boost::recursive_mutex *connectionMutex;
+    private: boost::recursive_mutex connectionMutex;
 
     /// \brief Mutex to protect msg bufferes.
-    private: boost::recursive_mutex *msgsMutex;
+    private: boost::recursive_mutex msgsMutex;
   };
 }
 #endif

File gazebo/Server.cc

 }
 
 /////////////////////////////////////////////////
-bool Server::OpenWorld(const std::string &_filename)
+bool Server::OpenWorld(const std::string & /*_filename*/)
 {
+  gzerr << "Open World is not implemented\n";
+  return false;
+/*
   sdf::SDFPtr sdf(new sdf::SDF);
   if (!sdf::init(sdf))
   {
   worldMsg.set_create(true);
   this->worldModPub->Publish(worldMsg);
   return true;
+  */
 }

File gazebo/common/Exception.cc

 
 //////////////////////////////////////////////////
 InternalError::InternalError(const char *_file, int _line,
-                             const std::string _msg) :
+                             const std::string &_msg) :
   Exception(_file, _line, _msg)
 {
 }
 //////////////////////////////////////////////////
 AssertionInternalError::AssertionInternalError(
     const char * _file, int _line,
-    const std::string _expr,
-    const std::string _function,
-    const std::string _msg) :
+    const std::string &_expr,
+    const std::string &_function,
+    const std::string &_msg) :
   InternalError(_file, _line,
       "GAZEBO ASSERTION                     \n" +
       _msg                               + "\n" +

File gazebo/common/Exception.hh

       /// \brief stream insertion operator for Gazebo Error
       /// \param[in] _out the output stream
       /// \param[in] _err the exception
-      public: friend std::ostream &operator<<(std::ostream& _out,
+      public: friend std::ostream &operator<<(std::ostream &_out,
                   const gazebo::common::Exception &_err)
               {
                 return _out << _err.GetErrorStr();
       /// \param[in] _line Line number where the error occurred
       /// \param[in] _msg Error message
       public: InternalError(const char *_file, int _line,
-                            const std::string _msg);
+                            const std::string &_msg);
 
       /// \brief Destructor
       public: virtual ~InternalError();
       /// \param[in] _msg Function where assertion failed
       public: AssertionInternalError(const char *_file,
                                      int _line,
-                                     const std::string _expr,
-                                     const std::string _function,
-                                     const std::string _msg = "");
+                                     const std::string &_expr,
+                                     const std::string &_function,
+                                     const std::string &_msg = "");
       /// \brief Destructor
       public: virtual ~AssertionInternalError();
     };

File gazebo/common/Mesh_TEST.cc

 */
 
 #include <gtest/gtest.h>
+#include <boost/filesystem.hpp>
 
 #include "test_config.h"
 #include "gazebo/math/Vector3.hh"
 /////////////////////////////////////////////////
 TEST(MeshTest, Mesh)
 {
+  // Cleanup test directory.
+  boost::filesystem::remove_all("/tmp/gazebo_test");
+  boost::filesystem::create_directories("/tmp/gazebo_test");
+
   EXPECT_EQ(NULL, common::MeshManager::Instance()->Load("break.mesh"));
   EXPECT_EQ(NULL, common::MeshManager::Instance()->Load("break.3ds"));
   EXPECT_EQ(NULL, common::MeshManager::Instance()->Load("break.xml"));
   newMesh->GenSphericalTexCoord(math::Vector3(0, 0, 0));
   delete newMesh;
 
-  std::ofstream stlFile("/tmp/gazebo_stl_test.stl", std::ios::out);
+  std::ofstream stlFile("/tmp/gazebo_test/gazebo_stl_test.stl", std::ios::out);
   stlFile << asciiSTLBox;
   stlFile.close();
 
-  mesh = common::MeshManager::Instance()->Load("/tmp/gazebo_stl_test-bad.stl");
+  mesh = common::MeshManager::Instance()->Load(
+      "/tmp/gazebo_test/gazebo_stl_test-bad.stl");
   EXPECT_EQ(NULL, mesh);
 
-  mesh = common::MeshManager::Instance()->Load("/tmp/gazebo_stl_test.stl");
+  mesh = common::MeshManager::Instance()->Load(
+      "/tmp/gazebo_test/gazebo_stl_test.stl");
   mesh->GetAABB(center, min, max);
   EXPECT_TRUE(center == math::Vector3(0.5, 0.5, 0.5));
   EXPECT_TRUE(min == math::Vector3(0, 0, 0));
   EXPECT_TRUE(max == math::Vector3(1, 1, 1));
+
+  // Cleanup test directory.
+  boost::filesystem::remove_all("/tmp/gazebo_test");
 }
 
 /////////////////////////////////////////////////

File gazebo/common/ModelDatabase.cc

     if (endIndex != std::string::npos)
       suffix = _uri.substr(endIndex, std::string::npos);
 
-    // store zip file in temp location
-    std::string filename = "/tmp/gz_model.tar.gz";
+    // Store downloaded .tar.gz and intermediate .tar files in temp location
+    boost::filesystem::path tmppath = boost::filesystem::temp_directory_path();
+    tmppath /= boost::filesystem::unique_path("gz_model-%%%%-%%%%-%%%%-%%%%");
+    std::string tarfilename = tmppath.string() + ".tar";
+    std::string tgzfilename = tarfilename + ".gz";
 
     CURL *curl = curl_easy_init();
     if (!curl)
       retry = false;
       iterations++;
 
-      FILE *fp = fopen(filename.c_str(), "wb");
+      FILE *fp = fopen(tgzfilename.c_str(), "wb");
       if (!fp)
       {
         gzerr << "Could not download model[" << _uri << "] because we were"
-          << "unable to write to file[" << filename << "]."
+          << "unable to write to file[" << tgzfilename << "]."
           << "Please fix file permissions.";
         return std::string();
       }
       try
       {
         // Unzip model tarball
-        std::ifstream file(filename.c_str(),
+        std::ifstream file(tgzfilename.c_str(),
             std::ios_base::in | std::ios_base::binary);
-        std::ofstream out("/tmp/gz_model.tar",
+        std::ofstream out(tarfilename.c_str(),
             std::ios_base::out | std::ios_base::binary);
         boost::iostreams::filtering_streambuf<boost::iostreams::input> in;
         in.push(boost::iostreams::gzip_decompressor());
       }
 
       TAR *tar;
-      tar_open(&tar, const_cast<char*>("/tmp/gz_model.tar"),
+      tar_open(&tar, const_cast<char*>(tarfilename.c_str()),
           NULL, O_RDONLY, 0644, TAR_GNU);
 
       std::string outputPath = getenv("HOME");
         << "The model may be corrupt.\n";
       path.clear();
     }
+
+    // Clean up
+    try
+    {
+      boost::filesystem::remove(tarfilename);
+      boost::filesystem::remove(tgzfilename);
+    }
+    catch(...)
+    {
+      gzwarn << "Failed to remove temporary model files after download.";
+    }
   }
 
   return path + suffix;

File gazebo/common/Plugin.hh

 #include <sys/types.h>
 #include <sys/stat.h>
 
-#include <gazebo_config.h>
+#include <gazebo/gazebo_config.h>
 #ifdef HAVE_DL
 #include <dlfcn.h>
 #elif HAVE_LTDL
               if (!found)
                 fullname = _filename;
 
+#ifdef HAVE_DL
               fptr_union_t registerFunc;
               std::string registerName = "RegisterPlugin";
 
-#ifdef HAVE_DL
               void* handle = dlopen(fullname.c_str(), RTLD_LAZY|RTLD_GLOBAL);
               if (!handle)
               {
               result.reset(registerFunc.func());
 
 #elif HAVE_LTDL
+              fptr_union_t registerFunc;
+              std::string registerName = "RegisterPlugin";
 
               static bool init_done = false;
 

File gazebo/common/Time.hh

       /// \param[in] _sec duration in seconds
       /// \return nanoseconds
       public: static inline double SecToNano(double _sec)
-              { return _sec * 1e-9;}
+              { return _sec * 1e9;}
 
       /// \brief Convert milliseconds to nanoseconds
       /// \param[in] _ms milliseconds
       /// \return nanoseconds
       public: static inline double MilToNano(double _ms)
-              { return _ms * 1e-6;}
+              { return _ms * 1e6;}
 
       /// \brief Convert microseconds to nanoseconds
       /// \param _ms microseconds
       /// \return nanoseconds
       public: static inline double MicToNano(double _ms)
-              { return _ms * 1e-3;}
+              { return _ms * 1e3;}
 
       /// \brief Stream insertion operator
       /// \param[in] _out the output stream

File gazebo/common/Time_TEST.cc

   time.Set(1, 1000);
   time = common::Time(1, 1000) / common::Time(2, 2);
   EXPECT_TRUE(time == common::Time(0, 500000499));
+
+  double sec = 1.0 + 1e-9;
+  double msec = sec * 1e3;
+  double usec = sec * 1e6;
+  double nsec = sec * 1e9;
+  EXPECT_DOUBLE_EQ(nsec, common::Time::SecToNano(sec));
+  EXPECT_DOUBLE_EQ(nsec, common::Time::MilToNano(msec));
+  EXPECT_DOUBLE_EQ(nsec, common::Time::MicToNano(usec));
 }
 
 

File gazebo/gui/CMakeLists.txt

 
 add_subdirectory(qtpropertybrowser)
 add_subdirectory(building)
+add_subdirectory(viewers)
 
 set (sources
   ../gazebo.cc
   TimePanel.cc
   ToolsWidget.cc
   TopicSelector.cc
-  viewers/ImageView.cc
-  viewers/ImagesView.cc
-  viewers/LaserView.cc
-  viewers/TopicView.cc
-  viewers/TextView.cc
-  viewers/ViewFactory.cc
 )
 
 set (qt_headers
   TimePanel.hh
   ToolsWidget.hh
   TopicSelector.hh
-  viewers/ImageView.hh
-  viewers/ImagesView.hh
-  viewers/LaserView.hh
-  viewers/TextView.hh
-  viewers/TopicView.hh
 )
 
 set (headers
   MeshMaker.hh
   ModelMaker.hh
   SphereMaker.hh
-  viewers/ViewFactory.hh
 )
 
 #if (HAVE_QWT)
 set (qt_tests
   DataLogger_TEST.cc
   TimePanel_TEST.cc
-  viewers/ImagesView_TEST.cc
 )
 
 # Generate executables for each of the QT unit tests
                                  gazebo_sensors
                                  gazebo_msgs
                                  gazebo_building
+                                 gazebo_gui_viewers
                                  gzqtpropertybrowser
                                  ${QT_LIBRARIES}
                                  ${ogre_libraries}
                                 gazebo_sensors
                                 gazebo_msgs
                                 gazebo_building
+                                gazebo_gui_viewers
                                 gzqtpropertybrowser
                                 ${QT_LIBRARIES}
                                 ${ogre_libraries}

File gazebo/gui/DataLogger_TEST.cc

 /////////////////////////////////////////////////
 void DataLogger_TEST::StressTest()
 {
+  // Cleanup test directory.
+  boost::filesystem::remove_all("/tmp/gazebo_test");
+
   this->Load("worlds/empty.world");
 
   gazebo::transport::NodePtr node;

File gazebo/gui/GuiEvents.cc

 event::EventT<void (const msgs::Model &)> Events::modelUpdate;
 event::EventT<void (const common::MouseEvent &)> Events::mousePress;
 event::EventT<void (const common::MouseEvent &)> Events::mouseRelease;
+event::EventT<void (int)> Events::inputStepSize;

File gazebo/gui/GuiEvents.hh

 
 
       //////////////////////////////////////////////////////////////////////////
-      /// \brief Connect a boost::slot the the move mode signal
+      /// \brief Connect a boost::slot to the move mode signal
       public: template<typename T>
               static event::ConnectionPtr ConnectMoveMode(T _subscriber)
               { return moveMode.Connect(_subscriber); }
               { moveMode.Disconnect(_subscriber); }
 
       //////////////////////////////////////////////////////////////////////////
-      /// \brief Connect a boost::slot the the manip mode signal
+      /// \brief Connect a boost::slot to the manip mode signal
       public: template<typename T>
               static event::ConnectionPtr ConnectManipMode(T _subscriber)
               {return manipMode.Connect(_subscriber);}
               {manipMode.Disconnect(_subscriber);}
 
       //////////////////////////////////////////////////////////////////////////
-      /// \brief Connect a boost::slot the the fullscreen signal
+      /// \brief Connect a boost::slot to the fullscreen signal
       public: template<typename T>
               static event::ConnectionPtr ConnectFullScreen(T _subscriber)
               { return fullScreen.Connect(_subscriber); }
       public: static void DisconnectFullScreen(event::ConnectionPtr _subscriber)
               { fullScreen.Disconnect(_subscriber); }
       //////////////////////////////////////////////////////////////////////////
-      /// \brief Connect a boost::slot the the view FPS signal
+      /// \brief Connect a boost::slot to the view FPS signal
       public: template<typename T>
               static event::ConnectionPtr ConnectFPS(T _subscriber)
               { return fps.Connect(_subscriber); }
       public: static void DisconnectFPS(event::ConnectionPtr _subscriber)
               { fps.Disconnect(_subscriber); }
       //////////////////////////////////////////////////////////////////////////
-      /// \brief Connect a boost::slot the the view Orbit signal
+      /// \brief Connect a boost::slot to the view Orbit signal
       public: template<typename T>
               static event::ConnectionPtr ConnectOrbit(T _subscriber)
               { return orbit.Connect(_subscriber); }
       public: static void DisconnectOrbit(event::ConnectionPtr _subscriber)
               { orbit.Disconnect(_subscriber); }
       //////////////////////////////////////////////////////////////////////////
-      /// \brief Connect a boost::slot the the view KeyPress signal
+      /// \brief Connect a boost::slot to the view KeyPress signal
       public: template<typename T>
               static event::ConnectionPtr ConnectKeyPress(T _subscriber)
               { return keyPress.Connect(_subscriber); }
                   event::ConnectionPtr _subscriber)
               { modelUpdate.Disconnect(_subscriber); }
 
+      //////////////////////////////////////////////////////////////////////////
+      /// \brief Connect a boost::slot to the input step size signal
+      public: template<typename T>
+              static event::ConnectionPtr ConnectInputStepSize(T _subscriber)
+              { return inputStepSize.Connect(_subscriber); }
+      public: static void DisconnectInputStepSize(
+              event::ConnectionPtr _subscriber)
+              { inputStepSize.Disconnect(_subscriber); }
+
       ///  that indicates the user is moving the camera
       public: static event::EventT<void (bool)>  moveMode;
 
               mousePress;
       public: static event::EventT<void (const common::MouseEvent &)>
               mouseRelease;
+
+      /// \brief Step size changed event
+      public: static event::EventT<void (int)> inputStepSize;
     };
   }
 }

File gazebo/gui/JointControlWidget.cc

  */
 #include "gazebo/transport/Node.hh"
 #include "gazebo/transport/Transport.hh"
+#include "gazebo/gui/Gui.hh"
 #include "gazebo/gui/JointControlWidget.hh"
 
 using namespace gazebo;
     this->jointPub = this->node->Advertise<msgs::JointCmd>(
         std::string("~/") + _modelName + "/joint_cmd");
 
-    boost::shared_ptr<msgs::Response> response = transport::request("default",
-        "entity_info", _modelName);
+    boost::shared_ptr<msgs::Response> response = transport::request(
+        gui::get_world(), "entity_info", _modelName);
 
     if (response->response() != "error" &&
         response->type() == modelMsg.GetTypeName())

File gazebo/gui/MainWindow.cc

   this->menuBar = NULL;
   this->setObjectName("mainWindow");
 
+  this->inputStepSize = 1;
   this->requestMsg = NULL;
   this->node = transport::NodePtr(new transport::Node());
   this->node->Init();
       gui::editor::Events::ConnectFinishBuildingModel(
       boost::bind(&MainWindow::OnFinishBuilding, this)));
 
+  this->connections.push_back(
+      gui::Events::ConnectInputStepSize(
+      boost::bind(&MainWindow::OnInputStepSizeChanged, this, _1)));
+
   gui::ViewFactory::RegisterAll();
 }
 
   msgs::WorldControl msg;
   msg.set_pause(false);
 
-  g_pauseAct->setChecked(false);
+  g_pauseAct->setVisible(true);
+  g_playAct->setVisible(false);
   this->worldControlPub->Publish(msg);
 }
 
   msgs::WorldControl msg;
   msg.set_pause(true);
 
-  g_playAct->setChecked(false);
+  g_pauseAct->setVisible(false);
+  g_playAct->setVisible(true);
   this->worldControlPub->Publish(msg);
 }
 
 void MainWindow::Step()
 {
   msgs::WorldControl msg;
-  msg.set_step(true);
+  msg.set_multi_step(this->inputStepSize);
 
   this->worldControlPub->Publish(msg);
 }
 
 /////////////////////////////////////////////////
+void MainWindow::OnInputStepSizeChanged(int _value)
+{
+  this->inputStepSize = _value;
+}
+
+/////////////////////////////////////////////////
 void MainWindow::NewModel()
 {
   /*ModelBuilderWidget *modelBuilder = new ModelBuilderWidget();
 void MainWindow::ShowGrid()
 {
   msgs::Scene msg;
-  msg.set_name("default");
+  msg.set_name(gui::get_world());
   msg.set_grid(g_showGridAct->isChecked());
   this->scenePub->Publish(msg);
 }
   g_editBuildingAct->setChecked(false);
   connect(g_editBuildingAct, SIGNAL(triggered()), this, SLOT(OnEditBuilding()));
 
+  g_stepAct = new QAction(QIcon(":/images/end.png"), tr("Step"), this);
+  g_stepAct->setStatusTip(tr("Step the world"));
+  connect(g_stepAct, SIGNAL(triggered()), this, SLOT(Step()));
+  QIcon icon = g_stepAct->icon();
+
+  // step action's icon is already in gray scale so there is no change in
+  // appearance when it is disabled. So create a custom semi-transparent
+  // icon for the disabled state.
+  QPixmap pixmap(":/images/end.png");
+  QPainter p(&pixmap);
+  p.setCompositionMode(QPainter::CompositionMode_DestinationIn);
+  p.fillRect(pixmap.rect(), QColor(0, 0, 0, 100));
+  icon.addPixmap(pixmap, QIcon::Disabled);
+  g_stepAct->setIcon(icon);
+
   g_playAct = new QAction(QIcon(":/images/play.png"), tr("Play"), this);
   g_playAct->setStatusTip(tr("Run the world"));
-  g_playAct->setCheckable(true);
-  g_playAct->setChecked(true);
+  g_playAct->setVisible(false);
   connect(g_playAct, SIGNAL(triggered()), this, SLOT(Play()));
+  connect(g_playAct, SIGNAL(changed()), this, SLOT(OnPlayActionChanged()));
+  this->OnPlayActionChanged();
 
   g_pauseAct = new QAction(QIcon(":/images/pause.png"), tr("Pause"), this);
   g_pauseAct->setStatusTip(tr("Pause the world"));
-  g_pauseAct->setCheckable(true);
-  g_pauseAct->setChecked(false);
+  g_pauseAct->setVisible(true);
   connect(g_pauseAct, SIGNAL(triggered()), this, SLOT(Pause()));
 
-  g_stepAct = new QAction(QIcon(":/images/end.png"), tr("Step"), this);
-  g_stepAct->setStatusTip(tr("Step the world"));
-  connect(g_stepAct, SIGNAL(triggered()), this, SLOT(Step()));
-
-
   g_arrowAct = new QAction(QIcon(":/images/arrow.png"),
       tr("Selection Mode"), this);
   g_arrowAct->setStatusTip(tr("Move camera"));
 /////////////////////////////////////////////////
 void MainWindow::OnStats(ConstWorldStatisticsPtr &_msg)
 {
-  if (_msg->paused() && g_playAct->isChecked())
+  if (_msg->paused() && g_pauseAct->isVisible())
   {
-    g_playAct->setChecked(false);
-    g_pauseAct->setChecked(true);
+    g_pauseAct->setVisible(false);
+    g_playAct->setVisible(true);
   }
-  else if (!_msg->paused() && !g_playAct->isChecked())
+  else if (!_msg->paused() && !g_playAct->isVisible())
   {
-    g_playAct->setChecked(true);
-    g_pauseAct->setChecked(false);
+    g_pauseAct->setVisible(true);
+    g_playAct->setVisible(false);
+  }
+}
+
+/////////////////////////////////////////////////
+void MainWindow::OnPlayActionChanged()
+{
+  if (g_playAct->isVisible())
+  {
+    g_stepAct->setToolTip("Step the world");
+    g_stepAct->setEnabled(true);
+  }
+  else
+  {
+    g_stepAct->setToolTip("Pause the world before stepping");
+    g_stepAct->setEnabled(false);
   }
 }
 

File gazebo/gui/MainWindow.hh

       private slots: void OnEditBuilding();
       private slots: void SetWireframe();
 
+      /// \brief Qt call back when the play action state changes
+      private slots: void OnPlayActionChanged();
+
       /// \brief Qt callback when the building editor's save action is
       /// triggered.
       private slots: void BuildingEditorSave();
       /// has been completed.
       private: void OnFinishBuilding();
 
+      /// \brief Handle event for changing the manual step size.
+      /// \param[in] _value New input step size.
+      private: void OnInputStepSizeChanged(int _value);
+
       private: QToolBar *playToolbar;
 
       private: RenderWidget *renderWidget;
       /// \brief The filename set via "Save As". This filename is used by
       /// the "Save" feature.
       private: std::string saveFilename;
+
+      /// \brief User specified step size for manually stepping the world
+      private: int inputStepSize;
     };
 
     class TreeViewDelegate: public QItemDelegate

File gazebo/gui/QTestFixture.cc

 #include "gazebo/gui/QTestFixture.hh"
 
 /////////////////////////////////////////////////
+QTestFixture::QTestFixture()
+  : server(NULL), serverThread(NULL), residentStart(0), shareStart(0)
+{
+}
+
+/////////////////////////////////////////////////
 void QTestFixture::initTestCase()
 {
   // Initialize the informational logger. This will log warnings, and
 /////////////////////////////////////////////////
 void QTestFixture::GetMemInfo(double &_resident, double &_share)
 {
+#ifdef __linux__
   int totalSize, residentPages, sharePages;
   totalSize = residentPages = sharePages = 0;
 
-#ifdef __linux__
   std::ifstream buffer("/proc/self/statm");
   buffer >> totalSize >> residentPages >> sharePages;
   buffer.close();

File gazebo/gui/QTestFixture.hh

 {
   Q_OBJECT
 
+  public: QTestFixture();
+
   /// \brief Load a world.
   /// \param[in] _worldFilename Name of the world to load.
   /// \param[in] _paused True to start the world paused.
   /// \brief Thread to run the Gazebo server.
   protected: boost::thread *serverThread;
 
+  /// \brief Resident memory at test start
   private: double residentStart;
+
+  /// \brief Shared memory at test start
   private: double shareStart;
 };
 

File gazebo/gui/RenderWidget.cc

 
   TimePanel *timePanel = new TimePanel(this);
 
-  QHBoxLayout *playControlLayout = new QHBoxLayout;
-  playControlLayout->setContentsMargins(0, 0, 0, 0);
-
   this->bottomFrame = new QFrame;
   this->bottomFrame->setObjectName("renderBottomFrame");
   this->bottomFrame->setSizePolicy(QSizePolicy::Expanding,
       QSizePolicy::Minimum);
 
+  QSpinBox *stepSpinBox = new QSpinBox;
+  stepSpinBox->setRange(1, 9999);
+  connect(stepSpinBox, SIGNAL(valueChanged(int)), this,
+      SLOT(OnStepValueChanged(int)));
+
+  QWidget *stepWidget = new QWidget;
+  QLabel *stepLabel = new QLabel(tr("Steps:"));
+  QVBoxLayout *stepLayout = new QVBoxLayout;
+  stepLayout->addWidget(stepLabel);
+  stepLayout->addWidget(stepSpinBox);
+  stepWidget->setLayout(stepLayout);
+
+  QLabel *stepToolBarLabel = new QLabel(tr("Steps:"));
+
+  QMenu *stepMenu = new QMenu;
+  this->stepButton = new QToolButton;
+  this->stepButton->setMaximumSize(35, stepButton->height());
+  QWidgetAction *stepAction = new QWidgetAction(stepMenu);
+  stepAction->setDefaultWidget(stepWidget);
+  stepMenu->addAction(stepAction);
+  this->stepButton->setMenu(stepMenu);
+  this->stepButton->setPopupMode(QToolButton::InstantPopup);
+  this->stepButton->setToolButtonStyle(Qt::ToolButtonTextOnly);
+  this->stepButton->setContentsMargins(0, 0, 0, 0);
+  this->OnStepValueChanged(1);
+
+  connect(stepSpinBox, SIGNAL(editingFinished()), stepMenu,
+      SLOT(hide()));
+
   QFrame *playFrame = new QFrame;
-  QToolBar *playToolbar = new QToolBar;
   playFrame->setFrameShape(QFrame::NoFrame);
   playFrame->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
   playFrame->setFixedHeight(25);
+  QToolBar *playToolbar = new QToolBar;
   playToolbar->addAction(g_playAct);
   playToolbar->addAction(g_pauseAct);
+
+  QLabel *emptyLabel = new QLabel(tr("  "));
+  playToolbar->addWidget(emptyLabel);
   playToolbar->addAction(g_stepAct);
+  playToolbar->addWidget(stepToolBarLabel);
+  playToolbar->addWidget(this->stepButton);
+
+  QHBoxLayout *playControlLayout = new QHBoxLayout;
+  playControlLayout->setContentsMargins(0, 0, 0, 0);
   playControlLayout->addWidget(playToolbar);
-  playControlLayout->setContentsMargins(0, 0, 0, 0);
+  playControlLayout->addItem(new QSpacerItem(15, -1, QSizePolicy::Expanding,
+                             QSizePolicy::Minimum));
   playFrame->setLayout(playControlLayout);
 
   bottomPanelLayout->addItem(new QSpacerItem(-1, -1, QSizePolicy::Expanding,
 {
   this->DisplayOverlayMsg("");
 }
+
+/////////////////////////////////////////////////
+void RenderWidget::OnStepValueChanged(int _value)
+{
+  // text formating and resizing for better presentation
+  std::string numStr = QString::number(_value).toStdString();
+  QFont stepFont = this->stepButton->font();
+  stepFont.setPointSizeF(11 - numStr.size()/2.0);
+  this->stepButton->setFont(stepFont);
+  numStr.insert(numStr.end(), 4 - numStr.size(), ' ');
+  this->stepButton->setText(tr(numStr.c_str()));
+
+  emit gui::Events::inputStepSize(_value);
+}

File gazebo/gui/RenderWidget.hh

       /// \return Message displayed in the render window
       public: std::string GetOverlayMsg() const;
 
+      /// \brief Qt call back when the step value in the spinbox changed
+      private slots: void OnStepValueChanged(int _value);
+
       private slots: virtual void update();
 
       /// \brief Qt callback to clear overlay message if a duration is
 
       /// \brief Base overlay message;
       private: std::string baseOverlayMsg;
+
+      /// \brief Tool button that holds the step widget
+      private: QToolButton *stepButton;
     };
   }
 }

File gazebo/gui/TimePanel.cc

   if (this->realTimes.size() > 20)
     this->realTimes.pop_front();
 
-  if (_msg->paused() && (g_pauseAct && !g_pauseAct->isChecked()))
+  if (_msg->paused() && (g_playAct && !g_playAct->isVisible()))
   {
-    g_pauseAct->setChecked(true);
-    g_playAct->setChecked(false);
+    g_playAct->setVisible(true);
+    g_pauseAct->setVisible(false);
   }
-  else if (!_msg->paused() && (g_playAct && !g_playAct->isChecked()))
+  else if (!_msg->paused() && (g_pauseAct && !g_pauseAct->isVisible()))
   {
-    g_pauseAct->setChecked(false);
-    g_playAct->setChecked(true);
+    g_pauseAct->setVisible(true);
+    g_playAct->setVisible(false);
   }
 
   unsigned int day, hour, min, sec, msec;

File gazebo/gui/building/BuildingMaker.cc

 
   // loop through all model manips
   for (itemsIt = this->allItems.begin(); itemsIt != this->allItems.end();
-      itemsIt++)
+      ++itemsIt)
   {
     visualNameStream.str("");
     collisionNameStream.str("");

File gazebo/gui/building/BuildingModelManip.cc

  *
 */
 
+#include "gazebo/rendering/Visual.hh"
 #include "gazebo/common/Exception.hh"
 #include "gazebo/math/Quaternion.hh"
 #include "gazebo/gui/building/BuildingMaker.hh"

File gazebo/gui/building/BuildingModelManip.hh

 #include <string>
 #include <vector>
 #include "gazebo/gui/qt.h"
-#include "gazebo/rendering/Visual.hh"
+#include "gazebo/math/Pose.hh"
+#include "gazebo/math/Vector3.hh"
+#include "gazebo/rendering/RenderTypes.hh"
 
 namespace gazebo
 {

File gazebo/gui/building/CMakeLists.txt

 QT4_WRAP_CPP(headers_MOC ${qt_headers})
 QT4_ADD_RESOURCES(resources_RCC ${resources})
 
-add_library(gazebo_building STATIC ${sources} ${headers_MOC} ${headers} ${resources_RCC})
+include_directories(${CMAKE_CURRENT_BINARY_DIR})
 
-include_directories(${CMAKE_CURRENT_BINARY_DIR})
+gz_add_library(gazebo_building ${sources} ${headers_MOC} ${headers} ${resources_RCC})
+
+target_link_libraries(gazebo_building gazebo_common
+                                      gazebo_transport
+                                      gazebo_sdf_interface
+                                      gazebo_rendering
+                                      gazebo_msgs)
+
+gz_install_library(gazebo_building)

File gazebo/gui/building/EditorView.cc

   this->levels.push_back(newlevel);
   emit gui::editor::Events::changeBuildingLevelName(this->currentLevel,
       levelName);
-  if (wallList.size() == 0)
+  if (this->wallList.empty())
   {
     newlevel->height = 0;
     return;
   double maxHeight = wallHeight;
   int wallLevel = 0;
 
-  wallIt++;
+  ++wallIt;
   for (wallIt; wallIt != this->wallList.end(); ++wallIt)
   {
     wallHeight = (*wallIt)->GetHeight() + (*wallIt)->GetLevelBaseHeight();

File gazebo/gui/building/FinishBuildingDialog.cc

 
   if (_mode == MODEL_FINISH)
     this->setWindowTitle(tr("Finish Model"));
-  else if (_mode == MODEL_FINISH)
+  else if (_mode == MODEL_SAVE)
     this->setWindowTitle(tr("Save Model"));
 
   QLabel *messageLabel = new QLabel;

File gazebo/gui/style.qss

+QToolButton
+{
+  border: 0;
+  text-align: left;
+  color: #d0d0d0;
+}
+
+QToolButton::disabled
+{
+  color: #303030;  
+}
+
 QLabel
 {
   color: #d0d0d0;
   color: white;
   font-size: 11px;
 }
-

File gazebo/gui/viewers/CMakeLists.txt

+include (${gazebo_cmake_dir}/GazeboUtils.cmake)
+include ( ${QT_USE_FILE} )
+
+
+set (sources
+  ImageView.cc
+  ImagesView.cc
+  LaserView.cc
+  TopicView.cc
+  TextView.cc
+  ViewFactory.cc
+)
+
+set (headers
+  ViewFactory.hh
+)
+
+set (qt_headers
+  ImageView.hh
+  ImagesView.hh
+  LaserView.hh
+  TextView.hh
+  TopicView.hh
+)
+
+set (qt_tests
+  ImagesView_TEST.cc
+)
+
+# Generate executables for each of the QT unit tests
+gz_build_qt_tests(${qt_tests})
+
+set (resources ../resources.qrc)
+QT4_WRAP_CPP(headers_MOC ${qt_headers})
+QT4_ADD_RESOURCES(resources_RCC ${resources})
+
+include_directories(${CMAKE_CURRENT_BINARY_DIR})
+
+gz_add_library(gazebo_gui_viewers ${sources}
+  ${headers_MOC} ${headers} ${resources_RCC})
+
+target_link_libraries(gazebo_gui_viewers
+  gazebo_common
+  gazebo_transport
+  gazebo_sdf_interface
+  gazebo_rendering
+  gazebo_msgs)
+
+gz_install_library(gazebo_gui_viewers)

File gazebo/gui/viewers/ImagesView.cc

       (*labelIter)->hide();
       this->frameLayout->removeWidget(*labelIter);
       delete *labelIter;
-      this->imageLabels.erase(labelIter);
+      labelIter = this->imageLabels.erase(labelIter);
     }
 
     // Clear the lists

File gazebo/gui/viewers/LaserView.cc

 
   this->laserItem->Clear();
 
-  double angle = _msg->scan().angle_min();
-
   double r;
   for (unsigned int i = 0;
        i < static_cast<unsigned int>(_msg->scan().ranges_size()); i++)
       this->laserItem->AddRange(r);
     else
       this->laserItem->SetRange(i+1, r);
-
-    angle += _msg->scan().angle_step();
   }
 
   // Recalculate the points to draw.
   if (index >= 0 && index < static_cast<int>(this->ranges.size()))
   {
     double x1, y1;
-    double x2, y2;
 
     double rangeScaled = this->ranges[index] * this->scale;
     double rangeMaxScaled = this->rangeMax * this->scale;
 
     // This section draws the arc and the angle of the ray
     {
+      double x2, y2;
       // Give the arc some padding.
       textWidth *= 1.4;
 

File gazebo/math/Matrix3.hh

       /// \param[in] _v The value to set in each row of the column
       public: void SetCol(unsigned int _c, const Vector3 &_v);
 
-      /// \brief Equality test operatorr
+      /// \brief returns the element wise difference of two matrices
+      public: Matrix3 operator-(const Matrix3 &_m) const
+      {
+        return Matrix3(
+        // first row
+        this->m[0][0]-_m[0][0], this->m[0][1]-_m[0][1], this->m[0][2]-_m[0][2],
+        this->m[1][0]-_m[1][0], this->m[1][1]-_m[1][1], this->m[1][2]-_m[1][2],
+        this->m[2][0]-_m[2][0], this->m[2][1]-_m[2][1], this->m[2][2]-_m[2][2]);
+      }
+
+      /// \brief returns the element wise sum of two matrices
+      public: Matrix3 operator+(const Matrix3 &_m) const
+      {
+        return Matrix3(
+        // first row
+        this->m[0][0]+_m[0][0], this->m[0][1]+_m[0][1], this->m[0][2]+_m[0][2],
+        this->m[1][0]+_m[1][0], this->m[1][1]+_m[1][1], this->m[1][2]+_m[1][2],
+        this->m[2][0]+_m[2][0], this->m[2][1]+_m[2][1], this->m[2][2]+_m[2][2]);
+      }
+
+      /// \brief returns the element wise scalar multiplication
+      public: Matrix3 operator*(const double &_s) const
+      {
+        return Matrix3(
+          // first row
+          _s * this->m[0][0], _s * this->m[0][1], _s * this->m[0][2],
+          _s * this->m[1][0], _s * this->m[1][1], _s * this->m[1][2],
+          _s * this->m[2][0], _s * this->m[2][1], _s * this->m[2][2]);
+      }
+
+      /// \brief Multiplication operators
+      /// \param[in] _s the scaling factor
+      /// \param[in] _m input matrix
+      /// \return a scaled matrix
+      public: friend inline Matrix3 operator*(double _s,
+                                              const Matrix3 &_m)
+      { return _m * _s; }
+
+      /// \brief Matrix multiplication operator
+      /// \param[in] _m Matrix3 to multiply
+      /// \return product of this * _m
+      public: Matrix3 operator*(const Matrix3 &_m) const
+      {
+        return Matrix3(
+          // first row
+          this->m[0][0]*_m[0][0]+this->m[0][1]*_m[1][0]+this->m[0][2]*_m[2][0],
+          this->m[0][0]*_m[0][1]+this->m[0][1]*_m[1][1]+this->m[0][2]*_m[2][1],
+          this->m[0][0]*_m[0][2]+this->m[0][1]*_m[1][2]+this->m[0][2]*_m[2][2],
+          // second row
+          this->m[1][0]*_m[0][0]+this->m[1][1]*_m[1][0]+this->m[1][2]*_m[2][0],
+          this->m[1][0]*_m[0][1]+this->m[1][1]*_m[1][1]+this->m[1][2]*_m[2][1],
+          this->m[1][0]*_m[0][2]+this->m[1][1]*_m[1][2]+this->m[1][2]*_m[2][2],
+          // third row
+          this->m[2][0]*_m[0][0]+this->m[2][1]*_m[1][0]+this->m[2][2]*_m[2][0],
+          this->m[2][0]*_m[0][1]+this->m[2][1]*_m[1][1]+this->m[2][2]*_m[2][1],
+          this->m[2][0]*_m[0][2]+this->m[2][1]*_m[1][2]+this->m[2][2]*_m[2][2]);
+      }
+
+      /// \brief Equality test operator
       /// \param[in] _m Matrix3 to test
       /// \return True if equal (using the default tolerance of 1e-6)
       public: bool operator==(const Matrix3 &_m) const;

File gazebo/math/Vector3.hh

       public: const Vector3 &operator*=(const Vector3 &_v);
 
       /// \brief Multiplication operators
-      /// \param[in] _v the scaling factor
+      /// \param[in] _s the scaling factor
+      /// \param[in] _v input vector
       /// \return a scaled vector
       public: friend inline Vector3 operator*(double _s,
                                               const Vector3 &_v)

File gazebo/msgs/msgs.hh

 
 #include <string>
 
-#include "msgs/MessageTypes.hh"
-#include "sdf/sdf.hh"
+#include "gazebo/msgs/MessageTypes.hh"
+#include "gazebo/sdf/sdf.hh"
 
-#include "math/MathTypes.hh"
-#include "math/Vector3.hh"
-#include "math/Pose.hh"
-#include "math/Plane.hh"
-#include "math/Box.hh"
+#include "gazebo/math/MathTypes.hh"
+#include "gazebo/math/Vector3.hh"
+#include "gazebo/math/Pose.hh"
+#include "gazebo/math/Plane.hh"
+#include "gazebo/math/Box.hh"
 
-#include "common/Color.hh"
-#include "common/Time.hh"
+#include "gazebo/common/Color.hh"
+#include "gazebo/common/Time.hh"
 
 namespace gazebo
 {

File gazebo/msgs/surface.proto

   optional double kd                        = 7;
   optional double max_vel                   = 8;
   optional double min_depth                 = 9;
+  optional bool collide_without_contact     = 10;
 }
 

File gazebo/msgs/world_control.proto

 {
   optional bool pause           = 1;
   optional bool step            = 2;
-  optional WorldReset reset     = 3;
-  optional uint32 seed          = 4;
+  optional uint32 multi_step    = 3;
+  optional WorldReset reset     = 4;
+  optional uint32 seed          = 5;
 }

File gazebo/physics/Base.cc

File contents unchanged.

File gazebo/physics/CMakeLists.txt

   World.hh
   WorldState.hh)
 
-set (gtest_sources
-  Joint_TEST.cc
-  PhysicsEngine_TEST.cc
-)
-gz_build_tests(${gtest_sources})
-
 set (physics_headers "" CACHE INTERNAL "physics headers" FORCE)
 foreach (hdr ${headers})
   APPEND_TO_CACHED_STRING(physics_headers
 gz_install_executable( gzphysics )
 gz_install_library(gazebo_physics)
 gz_install_includes("physics" ${headers} ${CMAKE_CURRENT_BINARY_DIR}/physics.hh)
+
+# unit tests
+set (gtest_sources
+  PhysicsEngine_TEST.cc
+  Inertial_TEST.cc
+  Joint_TEST.cc)
+gz_build_tests(${gtest_sources})

File gazebo/physics/Inertial.cc

   sdf::initFile("inertial.sdf", this->sdf);
 
   this->mass = _m;
-  this->cog.Set(0, 0, 0);
+  this->cog.Set(0, 0, 0, 0, 0, 0);
   this->principals.Set(1, 1, 1);
   this->products.Set(0, 0, 0);
 }
 {
   this->sdf = _sdf;
 
-  math::Vector3 center(0, 0, 0);
-  if (this->sdf->HasElement("pose"))
-  {
-    center = this->sdf->GetValuePose("pose").pos;
-  }
-  this->SetCoG(center.x, center.y, center.z);
+  // use default pose (identity) if not specified in sdf
+  math::Pose pose = this->sdf->GetValuePose("pose");
+  this->SetCoG(pose);
 
   // if (this->sdf->HasElement("inertia"))
   // Do the following whether an inertia element was specified or not.
 }
 
 //////////////////////////////////////////////////
+Inertial Inertial::GetInertial(const math::Pose &_frameOffset) const
+{
+  // make a copy of the current Inertial
+  Inertial result(*this);
+
+  // new CoG location after link frame offset
+  result.cog = result.cog - _frameOffset;
+
+  // new MOI after link frame offset
+  result.SetMOI(this->GetMOI(result.cog));
+
+  return result;
+}
+
+//////////////////////////////////////////////////
 void Inertial::Reset()
 {
   sdf::ElementPtr inertiaElem = this->sdf->GetElement("inertia");
 
   this->mass = this->sdf->GetValueDouble("mass");
-  this->cog.Set(0, 0, 0);
+  this->cog.Set(0, 0, 0, 0, 0, 0);
   this->SetInertiaMatrix(
         inertiaElem->GetValueDouble("ixx"),
         inertiaElem->GetValueDouble("iyy"),
 //////////////////////////////////////////////////
 void Inertial::SetCoG(double _cx, double _cy, double _cz)
 {
-  this->cog.Set(_cx, _cy, _cz);
+  this->cog.pos.Set(_cx, _cy, _cz);
 }
 
 //////////////////////////////////////////////////
 void Inertial::SetCoG(const math::Vector3 &_c)
 {
+  this->cog.pos = _c;
+}
+
+//////////////////////////////////////////////////
+void Inertial::SetCoG(double _cx, double _cy, double _cz,
+                      double _rx, double _ry, double _rz)
+{
+  this->cog.Set(_cx, _cy, _cz, _rx, _ry, _rz);
+}
+
+//////////////////////////////////////////////////
+void Inertial::SetCoG(const math::Pose &_c)
+{
   this->cog = _c;
 }
 
 }
 
 //////////////////////////////////////////////////
+void Inertial::SetMOI(const math::Matrix3 &_moi)
+{
+  /// \TODO: check symmetry of incoming _moi matrix
+  this->principals.Set(_moi[0][0], _moi[1][1], _moi[2][2]);
+  this->products.Set(_moi[0][1], _moi[0][2], _moi[1][2]);
+}
+
+//////////////////////////////////////////////////
+math::Matrix3 Inertial::GetMOI() const
+{
+  return math::Matrix3(
+    this->principals.x, this->products.x,   this->products.y,
+    this->products.x,   this->principals.y, this->products.z,
+    this->products.y,   this->products.z,   this->principals.z);
+}
+
+//////////////////////////////////////////////////
 void Inertial::Rotate(const math::Quaternion &_rot)
 {
-  this->cog = _rot.RotateVector(this->cog);
+  /// \TODO: double check what this does, if needed
+  this->cog.pos = _rot.RotateVector(this->cog.pos);
+  this->cog.rot = _rot * this->cog.rot;
 }
 
 //////////////////////////////////////////////////
 //////////////////////////////////////////////////
 Inertial Inertial::operator+(const Inertial &_inertial) const
 {
-  Inertial result;
+  Inertial result(*this);
+
+  // update mass with sum
   result.mass = this->mass + _inertial.mass;
 
-  result.cog = (this->cog*this->mass + _inertial.cog * _inertial.mass) /
-                result.mass;
+  // compute new center of mass
+  result.cog.pos =
+    (this->cog.pos*this->mass + _inertial.cog.pos * _inertial.mass) /
+    result.mass;
 
-  result.principals = this->principals + _inertial.principals;
-  result.products = this->products + _inertial.products;
+  // make a decision on the new orientation, set it to identity
+  result.cog.rot = math::Quaternion(1, 0, 0, 0);
+
+  // compute equivalent I for (*this) at the new CoG
+  math::Matrix3 Ithis = this->GetMOI(result.cog);
+
+  // compute equivalent I for _inertial at the new CoG
+  math::Matrix3 Iparam = _inertial.GetMOI(result.cog);
+
+  // sum up principals and products now they are at the same location
+  result.SetMOI(Ithis + Iparam);
+
   return result;
 }
 
 //////////////////////////////////////////////////
+math::Matrix3 Inertial::GetMOI(const math::Pose &_pose) const
+{
+  // get MOI as a Matrix3
+  math::Matrix3 moi = this->GetMOI();
+
+  // transform from new _pose to old this->cog, specified in new _pose frame
+  math::Pose new2Old = this->cog - _pose;
+
+  // rotate moi into new cog frame
+  moi = new2Old.rot.GetAsMatrix3() * moi *
+        new2Old.rot.GetInverse().GetAsMatrix3();
+
+  // parallel axis theorem to get MOI at the new cog location
+  // integrating point mass at some offset
+  math::Vector3 offset = new2Old.pos;
+  moi[0][0] += (offset.y * offset.y + offset.z * offset.z) * this->mass;
+  moi[0][1] -= (offset.x * offset.y) * this->mass;
+  moi[0][2] -= (offset.x * offset.z) * this->mass;
+  moi[1][0] -= (offset.y * offset.x) * this->mass;
+  moi[1][1] += (offset.x * offset.x + offset.z * offset.z) * this->mass;
+  moi[1][2] -= (offset.y * offset.z) * this->mass;
+  moi[2][0] -= (offset.z * offset.x) * this->mass;
+  moi[2][1] -= (offset.z * offset.y) * this->mass;
+  moi[2][2] += (offset.x * offset.x + offset.y * offset.y) * this->mass;
+
+  return moi;
+}
+
+//////////////////////////////////////////////////
 const Inertial &Inertial::operator+=(const Inertial &_inertial)
 {
   *this = *this + _inertial;

File gazebo/physics/Inertial.hh

 #include "gazebo/sdf/sdf.hh"
 #include "gazebo/math/Quaternion.hh"
 #include "gazebo/math/Vector3.hh"
+#include "gazebo/math/Matrix3.hh"
 
 namespace gazebo
 {
       public: double GetMass() const;
 
       /// \brief Set the mass matrix.
-      /// \param[in] _ixx X second moment of inertia about x axis.
+      /// \param[in] _ixx X second moment of inertia (MOI) about x axis.
       /// \param[in] _iyy Y second moment of inertia about y axis.
       /// \param[in] _izz Z second moment of inertia about z axis.
       /// \param[in] _ixy XY inertia.
       /// \param[in] _center Center of the gravity.
       public: void SetCoG(const math::Vector3 &_center);
 
+      /// \brief Set the center of gravity and rotation offset of inertial
+      ///        coordinate frame relative to Link frame.
+      /// \param[in] _cx Center offset in x-direction in Link frame
+      /// \param[in] _cy Center offset in y-direction in Link frame
+      /// \param[in] _cz Center offset in z-direction in Link frame
+      /// \param[in] _rx Roll angle offset of inertial coordinate frame.
+      /// \param[in] _ry Pitch angle offset of inertial coordinate frame.
+      /// \param[in] _rz Yaw angle offset of inertial coordinate frame.
+      public: void SetCoG(double _cx, double _cy, double _cz,
+                          double _rx, double _ry, double _rz);
+
+      /// \brief Set the center of gravity.
+      /// \param[in] _c Transform to center of gravity.
+      public: void SetCoG(const math::Pose &_c);
+
       /// \brief Get the center of gravity.
       /// \return The center of gravity.
       public: inline const math::Vector3 &GetCoG() const
-              {return this->cog;}
+              {
+                return this->cog.pos;
+              }
 
       /// \brief Get the pose about which the mass and inertia matrix is
       /// specified in the Link frame.
       /// \return The inertial pose.
       public: inline const math::Pose GetPose() const
-              { return math::Pose(this->cog, math::Quaternion());}
+              {
+                return math::Pose(this->cog);
+              }
 
       /// \brief Get the principal moments of inertia (Ixx, Iyy, Izz).
       /// \return The principal moments.
       public: math::Vector3 GetPrincipalMoments() const;
 
-      /// \brief Get the products of inertia (Ixy, Ixy, Iyz).
+      /// \brief Get the products of inertia (Ixy, Ixz, Iyz).
       /// \return The products of inertia.
       public: math::Vector3 GetProductsofInertia() const;
 
       public: Inertial &operator=(const Inertial &_inertial);
 
       /// \brief Addition operator.
+      /// Assuming both CG and Moment of Inertia (MOI) are defined
+      /// in the same reference Link frame.
+      /// New CG is computed from masses and perspective offsets,
+      /// and both MOI contributions relocated to the new cog.
       /// \param[in] _inertial Inertial to add.
       /// \return The result of the addition.
       public: Inertial operator+(const Inertial &_inertial) const;
       /// \param[in] _msg Message to read
       public: void ProcessMsg(const msgs::Inertial &_msg);
 
+      /// \brief Get the equivalent inertia from a point in local Link frame
+      /// If you specify GetMOI(this->GetPose()), you should get
+      /// back the Moment of Inertia (MOI) exactly as specified in the SDF.
+      /// If _pose is different from pose of the Inertial block, then
+      /// the MOI is rotated accordingly, and contributions from changes
+      /// in MOI location location due to point mass is added to the final MOI.
+      /// \param[in] _pose location in Link