John Hsu avatar John Hsu committed a98e310 Merge

merging from default

Comments (0)

Files changed (51)

 33bd3f7ab875e1ff2cf307e1042affd22f227d55 gazebo-prerelease_1.4.5
 f6c5e55b4a16b7c116a57abe5f0c46c528fb7ab0 gazebo-prerelease_1.4.5
 a6d1045e4d2a7f58350efe93e0883ce6e07dbcca gazebo-prereleaase_1.4.6
+cae37f25b4bfb1c86aecad208a732a17fa8df4af gazebo-prerelease_1.4.6
+069fbd92dfc7da3714e7be36115415ac1abe22c9 gazebo-prerelease_1.4.7
+3d3d262f1ac84d6789c34b913fb825c145ec7495 gazebo_1.4.0
 # 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 6)
+set (GAZEBO_PATCH_VERSION 0)
 
 set (GAZEBO_VERSION ${GAZEBO_MAJOR_VERSION}.${GAZEBO_MINOR_VERSION})
 set (GAZEBO_VERSION_FULL ${GAZEBO_MAJOR_VERSION}.${GAZEBO_MINOR_VERSION}.${GAZEBO_PATCH_VERSION})

cmake/GazeboUtils.cmake

   set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -Wl,-undefined -Wl,dynamic_lookup")
 endmacro()
 
+# This should be migrated to more fine control solution based on set_property APPEND
+# directories. It's present on cmake 2.8.8 while precise version is 2.8.7  
+link_directories(${PROJECT_BINARY_DIR}/test)
+include_directories("${PROJECT_SOURCE_DIR}/test/gtest/include")
+
 #################################################
+# Hack: extra sources to build binaries can be supplied to gz_build_tests in the variable
+#       GZ_BUILD_TESTS_EXTRA_EXE_SRCS. This variable will be clean up at the end of the function
 macro (gz_build_tests)
-
   # Build all the tests
   foreach(GTEST_SOURCE_file ${ARGN})
     string(REGEX REPLACE ".cc" "" BINARY_NAME ${GTEST_SOURCE_file})
-    add_executable(${BINARY_NAME} ${GTEST_SOURCE_file})
-
-    # This should be migrated to more fine control solution based on set_property APPEND
-    # directories. It's present on cmake 2.8.8 while precise version is 2.8.7  
-    include_directories("${PROJECT_SOURCE_DIR}/test/gtest/include")
+    add_executable(${BINARY_NAME} ${GTEST_SOURCE_file} ${GZ_BUILD_TESTS_EXTRA_EXE_SRCS})
 
     add_dependencies(${BINARY_NAME}
       gtest gtest_main
              ${CMAKE_BINARY_DIR}/test_results/${BINARY_NAME}.xml)
   endforeach()
 
+  set(GZ_BUILD_TESTS_EXTRA_EXE_SRCS "")
 endmacro()
 
 #################################################
-# INTERNAL function: do not call directly use gz_build_display_tests or gz_build_dri_tests
-macro (_gz_build_qt_tests)
-  # Build all the tests
-  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)
 
-    add_executable(${BINARY_NAME}
+# Define GUI testing macros as empty and redefine them if support is found
+macro (gz_build_qt_tests)
+endmacro()
+macro (gz_build_display_tests)
+endmacro()
+macro (gz_build_dri_tests)
+endmacro()
+
+if (VALID_DISPLAY)
+  # Redefine build display tests
+  macro (gz_build_display_tests)
+    gz_build_tests(${ARGV})
+  endmacro()
+
+  # Redefine build qt tests
+  macro (gz_build_qt_tests)
+   # Build all the tests
+   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)
+
+     add_executable(${BINARY_NAME}
       ${${BINARY_NAME}_MOC} ${QTEST_SOURCE_file} QTestFixture.cc)
 
     add_dependencies(${BINARY_NAME}
       ${QT_QTTEST_LIBRARY}
       ${QT_LIBRARIES}
       )
- 
+
     add_test(${BINARY_NAME} ${CMAKE_CURRENT_BINARY_DIR}/${BINARY_NAME} -xml)
-  
+
     set_tests_properties(${BINARY_NAME} PROPERTIES TIMEOUT 240)
-  
+
     # Check that the test produced a result and create a failure if it didn't.
     # Guards against crashed and timed out tests.
     add_test(check_${BINARY_NAME} ${PROJECT_SOURCE_DIR}/tools/check_test_ran.py
              ${CMAKE_BINARY_DIR}/test_results/${BINARY_NAME}.xml)
-  endforeach()
-endmacro()
-
-# Define GUI testing macros as empty and redefine them if support is found
-macro (gz_build_display_tests)
-endmacro()
-macro (gz_build_dri_tests)
-endmacro()
+    endforeach()
+  endmacro()
+endif()
 
 if (VALID_DRI_DISPLAY)
   macro (gz_build_dri_tests)
-    _gz_build_qt_tests(${ARGV})
+    gz_build_tests(${ARGV})
   endmacro()
 endif()
-
-if (VALID_DISPLAY)
-  macro (gz_build_display_tests)
-    _gz_build_qt_tests(${ARGV})
-  endmacro()
-endif()
-
-

cmake/SearchForStuff.cmake

 
   #################################################
   # Find bullet
-  pkg_check_modules(BULLET bullet)
+  pkg_check_modules(BULLET bullet>=2.81)
   if (BULLET_FOUND)
     set (HAVE_BULLET TRUE)
   else()

gazebo/gui/CMakeLists.txt

   viewers/ViewFactory.hh
 )
 
-set (display_tests
+set (qt_tests 
   DataLogger_TEST.cc
   TimePanel_TEST.cc
   viewers/ImagesView_TEST.cc
 )
 
-# Generate executables for each of the QT unit tests
-gz_build_display_tests(${tests})
+gz_build_qt_tests(${qt_tests})
 
 set (resources resources.qrc)
 
+
 QT4_WRAP_CPP(headers_MOC ${qt_headers})
 QT4_ADD_RESOURCES(resources_RCC ${resources})
 
 gz_add_executable(gzclient ${sources} main.cc ${headers_MOC} ${headers} ${resources_RCC})
-
 gz_add_library(gazebo_gui ${sources} ${headers_MOC} ${headers} ${resources_RCC})
 
 add_dependencies(gazebo_gui gazebo_msgs)

gazebo/gui/DataLogger.cc

 
 
       // Get the size units.
-      switch(_msg->log_file().size_units())
+      switch (_msg->log_file().size_units())
       {
         case msgs::LogStatus::LogFile::BYTES:
           stream << "B";

gazebo/gui/DataLogger_TEST.cc

 /////////////////////////////////////////////////
 void DataLogger_TEST::RecordButton()
 {
+  this->Load("worlds/empty.world");
+
   // Create a new data logger widget
   gazebo::gui::DataLogger *dataLogger = new gazebo::gui::DataLogger;
+  dataLogger->show();
+  QCoreApplication::processEvents();
 
   // Get the record button
   QToolButton *recordButton = dataLogger->findChild<QToolButton*>(
       "dataLoggerRecordButton");
 
   // Get the destination label
-  QLabel *destPathLabel = dataLogger->findChild<QLabel*>(
+  QLineEdit *destPathLabel = dataLogger->findChild<QLineEdit*>(
       "dataLoggerDestnationPathLabel");
 
   // Get the time label
   recordButton->toggle();
 
   // Wait for a log status return message
-  while (destPathLabel->text().toStdString() == "Path: ")
+  while (destPathLabel->text().toStdString().empty())
   {
     // The following line tell QT to process its events. This is vital for
     // all tests, but it must be run in the main thread.
   // Make sure the status label says "Ready"
   txt = statusLabel->text().toStdString();
   QVERIFY(txt == "Ready");
+
+  dataLogger->hide();
 }
 
 /////////////////////////////////////////////////
 void DataLogger_TEST::StressTest()
 {
+  this->Load("worlds/empty.world");
+
   gazebo::transport::NodePtr node;
   gazebo::transport::PublisherPtr pub;
 

gazebo/gui/QTestFixture.cc

   // errors.
   gazebo::common::Console::Instance()->Init("test.log");
 
+  // Initialize the data logger. This will log state information.
+  gazebo::common::LogRecord::Instance()->Init("test");
+
   // Add local search paths
   gazebo::common::SystemPaths::Instance()->AddGazeboPaths(PROJECT_SOURCE_PATH);
 

gazebo/physics/Joint.cc

   this->AddType(Base::JOINT);
   this->forceApplied[0] = 0;
   this->forceApplied[1] = 0;
+  this->effortLimit[0] = -1;
+  this->effortLimit[1] = -1;
+  this->velocityLimit[0] = -1;
+  this->velocityLimit[1] = -1;
 }
 
 //////////////////////////////////////////////////
       this->SetHighStop(0, limitElem->GetValueDouble("upper"));
       this->SetLowStop(0, limitElem->GetValueDouble("lower"));
       this->SetHighStop(0, limitElem->GetValueDouble("upper"));
+      this->effortLimit[0] = limitElem->GetValueDouble("effort");
+      this->velocityLimit[0] = limitElem->GetValueDouble("velocity");
     }
   }
 
       this->SetHighStop(1, limitElem->GetValueDouble("upper"));
       this->SetLowStop(1, limitElem->GetValueDouble("lower"));
       this->SetHighStop(1, limitElem->GetValueDouble("upper"));
+      this->effortLimit[1] = limitElem->GetValueDouble("effort");
+      this->velocityLimit[1] = limitElem->GetValueDouble("velocity");
     }
   }
 
 }
 
 //////////////////////////////////////////////////
+double Joint::GetEffortLimit(int _index)
+{
+  if (_index >= 0 && static_cast<unsigned int>(_index) < this->GetAngleCount())
+    return this->effortLimit[_index];
+
+  gzerr << "GetEffortLimit index[" << _index << "] out of range\n";
+  return 0;
+}
+
+//////////////////////////////////////////////////
+double Joint::GetVelocityLimit(int _index)
+{
+  if (_index >= 0 && static_cast<unsigned int>(_index) < this->GetAngleCount())
+    return this->velocityLimit[_index];
+
+  gzerr << "GetVelocityLimit index[" << _index << "] out of range\n";
+  return 0;
+}
+
+//////////////////////////////////////////////////
 void Joint::Update()
 {
   this->jointUpdate();
 //////////////////////////////////////////////////
 void Joint::SetForce(int _index, double _force)
 {
-  /// \todo: should check to see if this type of joint has _index
-  if (_index < 2)
+  // this bit of code actually doesn't do anything physical,
+  // it simply records the forces commanded inside forceApplied.
+  if (_index >= 0 && static_cast<unsigned int>(_index) < this->GetAngleCount())
     this->forceApplied[_index] = _force;
   else
-    gzerr << "Invalid joint index [" << _index
-          << "] when trying to apply force\n";
+    gzerr << "Something's wrong, joint [" << this->GetName()
+          << "] index [" << _index
+          << "] out of range.\n";
 }
 
 //////////////////////////////////////////////////
 double Joint::GetForce(int _index)
 {
-  /// \todo: should check to see if this type of joint has _index
-  if (_index < 2)
+  if (_index >= 0 && static_cast<unsigned int>(_index) < this->GetAngleCount())
+  {
     return this->forceApplied[_index];
+  }
   else
   {
     gzerr << "Invalid joint index [" << _index

gazebo/physics/Joint.hh

       /// \return Angle of the low stop value.
       public: virtual math::Angle GetLowStop(int _index) = 0;
 
+      /// \brief Get the effort limit on axis(index).
+      /// \param[in] _index Index of axis, where 0=first axis and 1=second axis
+      /// \return Effort limit specified in SDF
+      public: virtual double GetEffortLimit(int _index);
+
+      /// \brief Get the velocity limit on axis(index).
+      /// \param[in] _index Index of axis, where 0=first axis and 1=second axis
+      /// \return Velocity limit specified in SDF
+      public: virtual double GetVelocityLimit(int _index);
+
       /// \brief Set the velocity of an axis(index).
       /// \param[in] _index Index of the axis.
       /// \param[in] _vel Velocity.
       public: virtual void SetForce(int _index, double _force);
 
       /// \brief @todo: not yet implemented.
-      /// Get the internal forces at a this Joint.
+      /// Get the forces applied at this Joint.
       /// Note that the unit of force should be consistent with the rest
       /// of the simulation scales.  E.g.  if you are using
       /// metric units, the unit for force is Newtons.  If using
       /// \return The force applied to an axis.
       public: virtual double GetForce(int _index);
 
-      /// \brief get force torque values at a joint
+      /// \brief get internal force and torque values at a joint
+      /// Note that you must set
+      ///   <provide_feedback>true<provide_feedback>
+      /// in the joint sdf to use this.
       /// \param[in] _index Force and torque on child link if _index = 0
       /// and on parent link of _index = 1
       /// \return The force and torque at the joint
       /// equivalent of simulated force torque sensor reading
       /// Allocate a 2 vector in case hinge2 joint is used.
       protected: double forceApplied[2];
+
+      /// \brief Store Joint effort limit as specified in SDF
+      protected: double effortLimit[2];
+
+      /// \brief Store Joint velocity limit as specified in SDF
+      protected: double velocityLimit[2];
     };
     /// \}
   }

gazebo/physics/MultiRayShape.cc

 
   this->offset = this->collisionParent->GetRelativePose();
 
-  // Create and array of ray collisions
-  for (unsigned int j = 0; j < (unsigned int)vertSamples; j++)
+  // Create an array of ray collisions
+  for (unsigned int j = 0; j < (unsigned int)vertSamples; ++j)
   {
-    for (unsigned int i = 0; i < (unsigned int)horzSamples; i++)
+    for (unsigned int i = 0; i < (unsigned int)horzSamples; ++i)
     {
       yawAngle = (horzSamples == 1) ? 0 :
         i * yDiff / (horzSamples - 1) + horzMinAngle;

gazebo/physics/bullet/BulletHingeJoint.cc

 }
 
 //////////////////////////////////////////////////
-void BulletHingeJoint::SetForce(int /*_index*/, double _torque)
+void BulletHingeJoint::SetForce(int _index, double _effort)
 {
+  if (_index < 0 || static_cast<unsigned int>(_index) >= this->GetAngleCount())
+  {
+    gzerr << "Calling BulletHingeJoint::SetForce with an index ["
+          << _index << "] out of range\n";
+    return;
+  }
+
+  // truncating SetForce effort if velocity limit reached.
+  if (this->velocityLimit[_index] >= 0)
+  {
+    if (this->GetVelocity(_index) > this->velocityLimit[_index])
+      _effort = _effort > 0 ? 0 : _effort;
+    else if (this->GetVelocity(_index) < -this->velocityLimit[_index])
+      _effort = _effort < 0 ? 0 : _effort;
+  }
+
+  // truncate effort unless effortLimit is negative.
+  if (this->effortLimit[_index] >= 0)
+    _effort = math::clamp(_effort, -this->effortLimit[_index],
+       this->effortLimit[_index]);
+
   // math::Vector3 axis = this->GetLocalAxis(_index);
   // this->bulletHinge->enableAngularMotor(true);
 
     this->bulletHinge->getRigidBodyA().getWorldTransform().getBasis() *
     hingeAxisLocal;
 
-  btVector3 hingeTorque = _torque * hingeAxisWorld;
+  btVector3 hingeTorque = _effort * hingeAxisWorld;
 
   this->bulletHinge->getRigidBodyA().applyTorque(hingeTorque);
   this->bulletHinge->getRigidBodyB().applyTorque(-hingeTorque);

gazebo/physics/bullet/BulletHingeJoint.hh

       public: virtual double GetMaxForce(int _index);
 
       // Documentation inherited.
-      public: virtual void SetForce(int _index, double _torque);
+      public: virtual void SetForce(int _index, double _effort);
 
       // Documentation inherited.
       public: virtual double GetForce(int _index);

gazebo/physics/bullet/BulletLink.cc

   if (mass <= 0.0)
     this->rigidLink->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT);
 
-  btDynamicsWorld *wd = this->bulletPhysics->GetDynamicsWorld();
-  wd->addRigidBody(this->rigidLink);
-
+  btDynamicsWorld *bulletWorld = this->bulletPhysics->GetDynamicsWorld();
+  GZ_ASSERT(bulletWorld != NULL, "Bullet dynamics world is NULL");
+  bulletWorld->addRigidBody(this->rigidLink);
   // this->rigidLink->setSleepingThresholds(0,0);
 }
 
 void BulletLink::Fini()
 {
   Link::Fini();
+  btDynamicsWorld *bulletWorld = this->bulletPhysics->GetDynamicsWorld();
+  GZ_ASSERT(bulletWorld != NULL, "Bullet dynamics world is NULL");
+  bulletWorld->removeRigidBody(this->rigidLink);
 }
 
 //////////////////////////////////////////////////

gazebo/physics/bullet/BulletMultiRayShape.cc

  * limitations under the License.
  *
  */
-#include "physics/World.hh"
-#include "physics/bullet/BulletTypes.hh"
-#include "physics/bullet/BulletLink.hh"
-#include "physics/bullet/BulletCollision.hh"
-#include "physics/bullet/BulletPhysics.hh"
-#include "physics/bullet/BulletRayShape.hh"
-#include "physics/bullet/BulletMultiRayShape.hh"
+
+#include "gazebo/common/Exception.hh"
+
+#include "gazebo/physics/World.hh"
+#include "gazebo/physics/bullet/BulletTypes.hh"
+#include "gazebo/physics/bullet/BulletLink.hh"
+#include "gazebo/physics/bullet/BulletCollision.hh"
+#include "gazebo/physics/bullet/BulletPhysics.hh"
+#include "gazebo/physics/bullet/BulletRayShape.hh"
+#include "gazebo/physics/bullet/BulletMultiRayShape.hh"
 
 using namespace gazebo;
 using namespace physics;
 //////////////////////////////////////////////////
 void BulletMultiRayShape::UpdateRays()
 {
-  std::vector< RayShapePtr >::iterator iter;
+  std::vector<RayShapePtr>::iterator iter;
   for (iter = this->rays.begin(); iter != this->rays.end(); ++iter)
   {
     (*iter)->Update();
     const math::Vector3 &_end)
 {
   MultiRayShape::AddRay(_start, _end);
-  BulletRayShapePtr ray(new BulletRayShape(this->physicsEngine));
+
+  BulletRayShapePtr ray(new BulletRayShape(this->collisionParent));
   ray->SetPoints(_start, _end);
 
   this->rays.push_back(ray);

gazebo/physics/bullet/BulletPhysics.cc

 void BulletPhysics::UpdatePhysics()
 {
   // need to lock, otherwise might conflict with world resetting
-  this->physicsUpdateMutex->lock();
+  boost::recursive_mutex::scoped_lock lock(*this->physicsUpdateMutex);
 
   // common::Time currTime =  this->world->GetRealTime();
 
   this->dynamicsWorld->stepSimulation(
       this->stepTimeDouble, 1, this->stepTimeDouble);
   // this->lastUpdateTime = currTime;
-
-  this->physicsUpdateMutex->unlock();
 }
 
 //////////////////////////////////////////////////

gazebo/physics/bullet/BulletRayShape.cc

  * Date: 24 May 2009
  */
 
-#include "physics/World.hh"
-#include "physics/bullet/BulletLink.hh"
-#include "physics/bullet/BulletPhysics.hh"
-#include "physics/bullet/BulletTypes.hh"
-#include "physics/bullet/BulletCollision.hh"
-#include "physics/bullet/BulletRayShape.hh"
+#include "gazebo/common/Assert.hh"
+
+#include "gazebo/physics/World.hh"
+#include "gazebo/physics/bullet/BulletLink.hh"
+#include "gazebo/physics/bullet/BulletPhysics.hh"
+#include "gazebo/physics/bullet/BulletTypes.hh"
+#include "gazebo/physics/bullet/BulletCollision.hh"
+#include "gazebo/physics/bullet/BulletRayShape.hh"
 
 using namespace gazebo;
 using namespace physics;
 
 //////////////////////////////////////////////////
 BulletRayShape::BulletRayShape(PhysicsEnginePtr _physicsEngine)
-  : RayShape(_physicsEngine),
-    rayCallback(btVector3(0, 0, 0), btVector3(0, 0, 0))
+  : RayShape(_physicsEngine)
 {
   this->SetName("Bullet Ray Shape");
 
 
 //////////////////////////////////////////////////
 BulletRayShape::BulletRayShape(CollisionPtr _parent)
-    : RayShape(_parent),
-    rayCallback(btVector3(0, 0, 0), btVector3(0, 0, 0))
+    : RayShape(_parent)
 {
   this->SetName("Bullet Ray Shape");
   this->physicsEngine = boost::shared_static_cast<BulletPhysics>(
 //////////////////////////////////////////////////
 void BulletRayShape::Update()
 {
+  if (this->collisionParent)
+  {
+    BulletCollisionPtr collision =
+      boost::shared_static_cast<BulletCollision>(this->collisionParent);
+
+    LinkPtr link = this->collisionParent->GetLink();
+    GZ_ASSERT(link != NULL, "Bullet link is NULL");
+
+    this->globalStartPos = link->GetWorldPose().CoordPositionAdd(
+          this->relativeStartPos);
+
+    this->globalEndPos = link->GetWorldPose().CoordPositionAdd(
+          this->relativeEndPos);
+  }
+
+  btVector3 start(this->globalStartPos.x, this->globalStartPos.y,
+      this->globalStartPos.z);
+  btVector3 end(this->globalEndPos.x, this->globalEndPos.y,
+      this->globalEndPos.z);
+
+  btCollisionWorld::ClosestRayResultCallback rayCallback(start, end);
+
+  boost::recursive_mutex::scoped_lock lock(
+      *this->physicsEngine->GetPhysicsUpdateMutex());
+
   this->physicsEngine->GetDynamicsWorld()->rayTest(
-      this->rayCallback.m_rayFromWorld,
-      this->rayCallback.m_rayToWorld,
-      this->rayCallback);
+      start, end, rayCallback);
 
-    if (this->rayCallback.hasHit())
-    {
-      math::Vector3 result(this->rayCallback.m_hitPointWorld.getX(),
-                           this->rayCallback.m_hitPointWorld.getY(),
-                           this->rayCallback.m_hitPointWorld.getZ());
-      this->SetLength(this->globalStartPos.Distance(result));
-    }
+  if (rayCallback.hasHit())
+  {
+    math::Vector3 result(rayCallback.m_hitPointWorld.getX(),
+                         rayCallback.m_hitPointWorld.getY(),
+                         rayCallback.m_hitPointWorld.getZ());
+    this->SetLength(this->globalStartPos.Distance(result));
+  }
 }
 
 //////////////////////////////////////////////////
 
   if (this->physicsEngine && this->collisionParent)
   {
+    btVector3 start(this->globalStartPos.x, this->globalStartPos.y,
+        this->globalStartPos.z);
+    btVector3 end(this->globalEndPos.x, this->globalEndPos.y,
+        this->globalEndPos.z);
+
+    btCollisionWorld::ClosestRayResultCallback rayCallback(start, end);
     this->physicsEngine->GetDynamicsWorld()->rayTest(
-        this->rayCallback.m_rayFromWorld,
-        this->rayCallback.m_rayToWorld,
-        this->rayCallback);
+        start, end, rayCallback);
+    if (rayCallback.hasHit())
+    {
+      math::Vector3 result(rayCallback.m_hitPointWorld.getX(),
+                           rayCallback.m_hitPointWorld.getY(),
+                           rayCallback.m_hitPointWorld.getZ());
+      _dist = this->globalStartPos.Distance(result);
 
-    if (this->rayCallback.hasHit())
-    {
-      math::Vector3 result(this->rayCallback.m_hitPointWorld.getX(),
-                           this->rayCallback.m_hitPointWorld.getY(),
-                           this->rayCallback.m_hitPointWorld.getZ());
-      _dist = this->globalStartPos.Distance(result);
-      _entity = static_cast<BulletLink*>(
-          this->rayCallback.m_collisionObject->getUserPointer())->GetName();
+      BulletLink *link = static_cast<BulletLink *>(
+          rayCallback.m_collisionObject->getUserPointer());
+      GZ_ASSERT(link != NULL, "Bullet link is NULL");
+      _entity = link->GetName();
     }
   }
 }
 void BulletRayShape::SetPoints(const math::Vector3 &_posStart,
                                    const math::Vector3 &_posEnd)
 {
-  this->globalStartPos = _posStart;
-  this->globalEndPos = _posEnd;
-
-  this->rayCallback.m_rayFromWorld.setX(_posStart.x);
-  this->rayCallback.m_rayFromWorld.setY(_posStart.y);
-  this->rayCallback.m_rayFromWorld.setZ(_posStart.z);
-
-  this->rayCallback.m_rayToWorld.setX(_posEnd.x);
-  this->rayCallback.m_rayToWorld.setY(_posEnd.y);
-  this->rayCallback.m_rayToWorld.setZ(_posEnd.z);
+  RayShape::SetPoints(_posStart, _posEnd);
 }

gazebo/physics/bullet/BulletRayShape.hh

       public: void SetPoints(const math::Vector3 &_posStart,
                              const math::Vector3 &_posEnd);
 
+      /// \brief Pointer to the Bullet physics engine
       private: BulletPhysicsPtr physicsEngine;
-      private: btCollisionWorld::ClosestRayResultCallback rayCallback;
     };
     /// \}
   }

gazebo/physics/bullet/BulletSliderJoint.cc

 }
 
 //////////////////////////////////////////////////
-void BulletSliderJoint::SetForce(int /*_index*/, double _force)
+void BulletSliderJoint::SetForce(int _index, double _effort)
 {
+  if (_index < 0 || static_cast<unsigned int>(_index) >= this->GetAngleCount())
+  {
+    gzerr << "Calling BulletSliderJoint::SetForce with an index ["
+          << _index << "] out of range\n";
+    return;
+  }
+
+  // truncating SetForce effort if velocity limit reached.
+  if (this->velocityLimit[_index] >= 0)
+  {
+    if (this->GetVelocity(_index) > this->velocityLimit[_index])
+      _effort = _effort > 0 ? 0 : _effort;
+    else if (this->GetVelocity(_index) < -this->velocityLimit[_index])
+      _effort = _effort < 0 ? 0 : _effort;
+  }
+
+  // truncate effort if effortLimit is not negative
+  if (this->effortLimit[_index] >= 0)
+    _effort = math::clamp(_effort, -this->effortLimit[_index],
+       this->effortLimit[_index]);
+
   /*btVector3 hingeAxisLocal = this->bulletSlider->getAFrame().getBasis().getColumn(2); // z-axis of constraint frame
   btVector3 hingeAxisWorld = this->bulletSlider->getRigidBodyA().getWorldTransform().getBasis() * hingeAxisLocal;
 
   btVector3 hingeTorque = _torque * hingeAxisWorld;
   */
 
-  btVector3 force(0, 0, _force);
+  btVector3 force(0, 0, _effort);
   this->constraint->getRigidBodyA().applyCentralForce(force);
   this->constraint->getRigidBodyB().applyCentralForce(-force);
 }

gazebo/physics/bullet/BulletSliderJoint.hh

       public: virtual void SetVelocity(int _index, double _angle);
 
       /// \brief Set the slider force
-      public: virtual void SetForce(int _index, double _force);
+      public: virtual void SetForce(int _index, double _effort);
 
       /// \brief Set the max allowed force of an axis(index).
-      public: virtual void SetMaxForce(int _index, double _t);
+      public: virtual void SetMaxForce(int _index, double _force);
 
       /// \brief Get the max allowed force of an axis(index).
       public: virtual double GetMaxForce(int _index);

gazebo/physics/bullet/BulletTrimeshShape.cc

     mTriMesh->addTriangle(bv0, bv1, bv2);
   }
 
-  bParent->SetCollisionShape(new btConvexTriangleMeshShape(mTriMesh, true));
+  btConvexShape* convexShape = new btConvexTriangleMeshShape(mTriMesh, true);
+  convexShape->setMargin(0.001f);
+  bParent->SetCollisionShape(convexShape);
 
   delete [] vertices;
   delete [] indices;

gazebo/physics/ode/ODEHinge2Joint.cc

 
 
 //////////////////////////////////////////////////
-void ODEHinge2Joint::SetForce(int _index, double _torque)
+void ODEHinge2Joint::SetForce(int _index, double _effort)
 {
-  ODEJoint::SetForce(_index, _torque);
+  if (_index < 0 || static_cast<unsigned int>(_index) >= this->GetAngleCount())
+  {
+    gzerr << "Calling BulletHingeJoint::SetForce with an index ["
+          << _index << "] out of range\n";
+    return;
+  }
+
+  // truncating SetForce effort if velocity limit reached.
+  if (this->velocityLimit[_index] >= 0)
+  {
+    if (this->GetVelocity(_index) > this->velocityLimit[_index])
+      _effort = _effort > 0 ? 0 : _effort;
+    else if (this->GetVelocity(_index) < -this->velocityLimit[_index])
+      _effort = _effort < 0 ? 0 : _effort;
+  }
+
+  // truncate effort if effortLimit is not negative
+  if (this->effortLimit[_index] >= 0)
+    _effort = math::clamp(_effort,
+      -this->effortLimit[_index], this->effortLimit[_index]);
+
+  ODEJoint::SetForce(_index, _effort);
   if (this->childLink)
     this->childLink->SetEnabled(true);
   if (this->parentLink)
     this->parentLink->SetEnabled(true);
 
   if (_index == 0)
-    dJointAddHinge2Torques(this->jointId, _torque, 0);
+    dJointAddHinge2Torques(this->jointId, _effort, 0);
   else
-    dJointAddHinge2Torques(this->jointId, 0, _torque);
+    dJointAddHinge2Torques(this->jointId, 0, _effort);
 }
 
 //////////////////////////////////////////////////

gazebo/physics/ode/ODEHinge2Joint.hh

       public: virtual double GetMaxForce(int index);
 
       // Documentation inherited.
-      public: virtual void SetForce(int index, double torque);
+      public: virtual void SetForce(int _index, double _effort);
 
       // Documentation inherited.
       public: virtual double GetParam(int parameter) const;

gazebo/physics/ode/ODEHingeJoint.cc

 }
 
 //////////////////////////////////////////////////
-void ODEHingeJoint::SetForce(int _index, double _torque)
+void ODEHingeJoint::SetForce(int _index, double _effort)
 {
-  ODEJoint::SetForce(_index, _torque);
+  if (_index < 0 || static_cast<unsigned int>(_index) >= this->GetAngleCount())
+  {
+    gzerr << "Calling ODEHingeJoint::SetForce with an index ["
+          << _index << "] out of range\n";
+    return;
+  }
+
+  // truncating SetForce effort if velocity limit reached.
+  if (this->velocityLimit[_index] >= 0)
+  {
+    if (this->GetVelocity(_index) > this->velocityLimit[_index])
+      _effort = _effort > 0 ? 0 : _effort;
+    else if (this->GetVelocity(_index) < -this->velocityLimit[_index])
+      _effort = _effort < 0 ? 0 : _effort;
+  }
+
+  // truncate effort if effortLimit is not negative
+  if (this->effortLimit[_index] >= 0)
+    _effort = math::clamp(_effort,
+      -this->effortLimit[_index], this->effortLimit[_index]);
+
+  ODEJoint::SetForce(_index, _effort);
   if (this->childLink)
     this->childLink->SetEnabled(true);
   if (this->parentLink)
     this->parentLink->SetEnabled(true);
-  dJointAddHingeTorque(this->jointId, _torque);
+
+  dJointAddHingeTorque(this->jointId, _effort);
 }
 
 //////////////////////////////////////////////////

gazebo/physics/ode/ODEHingeJoint.hh

       public: virtual double GetMaxForce(int _index);
 
       // Documentation inherited
-      public: virtual void SetForce(int _index, double _torque);
+      public: virtual void SetForce(int _index, double _effort);
 
       // Documentation inherited
       public: virtual double GetParam(int _parameter) const;

gazebo/physics/ode/ODEScrewJoint.cc

 }
 
 //////////////////////////////////////////////////
-void ODEScrewJoint::SetForce(int _index, double _force)
+void ODEScrewJoint::SetForce(int _index, double _effort)
 {
-  ODEJoint::SetForce(_index, _force);
+  if (_index < 0 || static_cast<unsigned int>(_index) >= this->GetAngleCount())
+  {
+    gzerr << "Calling ODEScrewJoint::SetForce with an index ["
+          << _index << "] out of range\n";
+    return;
+  }
+
+  // truncating SetForce effort if velocity limit reached.
+  if (this->velocityLimit[_index] >= 0)
+  {
+    if (this->GetVelocity(_index) > this->velocityLimit[_index])
+      _effort = _effort > 0 ? 0 : _effort;
+    else if (this->GetVelocity(_index) < -this->velocityLimit[_index])
+      _effort = _effort < 0 ? 0 : _effort;
+  }
+
+  // truncate effort if effortLimit is not negative
+  if (this->effortLimit[_index] >= 0.0)
+    _effort = math::clamp(_effort,
+      -this->effortLimit[_index], this->effortLimit[_index]);
+
+  ODEJoint::SetForce(_index, _effort);
   if (this->childLink) this->childLink->SetEnabled(true);
   if (this->parentLink) this->parentLink->SetEnabled(true);
-  // dJointAddScrewForce(this->jointId, _force);
-  dJointAddScrewTorque(this->jointId, _force);
+
+  // dJointAddScrewForce(this->jointId, _effort);
+  dJointAddScrewTorque(this->jointId, _effort);
 }
 
 //////////////////////////////////////////////////

gazebo/physics/ode/ODEScrewJoint.hh

       public: virtual void SetVelocity(int _index, double _angle);
 
       // Documentation inherited
-      public: virtual void SetForce(int _index, double _force);
+      public: virtual void SetForce(int _index, double _effort);
 
       // Documentation inherited
       public: virtual void SetMaxForce(int _index, double _t);

gazebo/physics/ode/ODESliderJoint.cc

 }
 
 //////////////////////////////////////////////////
-void ODESliderJoint::SetForce(int _index, double _force)
+void ODESliderJoint::SetForce(int _index, double _effort)
 {
-  ODEJoint::SetForce(_index, _force);
+  if (_index < 0 || static_cast<unsigned int>(_index) >= this->GetAngleCount())
+  {
+    gzerr << "Calling ODEScrewJoint::SetForce with an index ["
+          << _index << "] out of range\n";
+    return;
+  }
+
+  // truncating SetForce effort if velocity limit reached.
+  if (this->velocityLimit[_index] >= 0)
+  {
+    if (this->GetVelocity(_index) > this->velocityLimit[_index])
+      _effort = _effort > 0 ? 0 : _effort;
+    else if (this->GetVelocity(_index) < -this->velocityLimit[_index])
+      _effort = _effort < 0 ? 0 : _effort;
+  }
+
+  // truncate effort if effortLimit is not negative
+  if (this->effortLimit[_index] >= 0.0)
+    _effort = math::clamp(_effort, -this->effortLimit[_index],
+      this->effortLimit[_index]);
+
+  ODEJoint::SetForce(_index, _effort);
   if (this->childLink)
     this->childLink->SetEnabled(true);
   if (this->parentLink)
     this->parentLink->SetEnabled(true);
 
-  dJointAddSliderForce(this->jointId, _force);
+  dJointAddSliderForce(this->jointId, _effort);
 }
 
 //////////////////////////////////////////////////

gazebo/physics/ode/ODESliderJoint.hh

       public: virtual void SetVelocity(int _index, double _angle);
 
       // Documentation inherited
-      public: virtual void SetForce(int _index, double _force);
+      public: virtual void SetForce(int _index, double _effort);
 
       // Documentation inherited
       public: virtual void SetMaxForce(int _index, double _t);

gazebo/physics/ode/ODEUniversalJoint.cc

 }
 
 //////////////////////////////////////////////////
-void ODEUniversalJoint::SetForce(int _index, double _torque)
+void ODEUniversalJoint::SetForce(int _index, double _effort)
 {
-  ODEJoint::SetForce(_index, _torque);
+  if (_index < 0 || static_cast<unsigned int>(_index) >= this->GetAngleCount())
+  {
+    gzerr << "Calling ODEUniversalJoint::SetForce with an index ["
+          << _index << "] out of range\n";
+    return;
+  }
+
+  // truncating SetForce effort if velocity limit reached.
+  if (this->velocityLimit[_index] >= 0)
+  {
+    if (this->GetVelocity(_index) > this->velocityLimit[_index])
+      _effort = _effort > 0 ? 0 : _effort;
+    else if (this->GetVelocity(_index) < -this->velocityLimit[_index])
+      _effort = _effort < 0 ? 0 : _effort;
+  }
+
+  // truncate effort if effortLimit is not negative
+  if (this->effortLimit[_index] >= 0.0)
+    _effort = math::clamp(_effort, -this->effortLimit[_index],
+      this->effortLimit[_index]);
+
+  ODEJoint::SetForce(_index, _effort);
   if (this->childLink) this->childLink->SetEnabled(true);
   if (this->parentLink) this->parentLink->SetEnabled(true);
+
   if (_index == 0)
-    dJointAddUniversalTorques(this->jointId, _torque, 0);
+    dJointAddUniversalTorques(this->jointId, _effort, 0);
   else
-    dJointAddUniversalTorques(this->jointId, 0, _torque);
+    dJointAddUniversalTorques(this->jointId, 0, _effort);
 }
 
 //////////////////////////////////////////////////

gazebo/physics/ode/ODEUniversalJoint.hh

       public: virtual void SetVelocity(int _index, double _angle);
 
       // Documentation inherited
-      public: virtual void SetForce(int _index, double _torque);
+      public: virtual void SetForce(int _index, double _effort);
 
       // Documentation inherited
       public: virtual void SetMaxForce(int _index, double _t);

gazebo/rendering/Heightmap.cc

 Heightmap::Heightmap(ScenePtr _scene)
 {
   this->scene = _scene;
+  this->terrainGlobals = NULL;
 }
 
 //////////////////////////////////////////////////
 //////////////////////////////////////////////////
 void Heightmap::Load()
 {
+  if (this->terrainGlobals != NULL)
+    return;
+
   this->terrainGlobals = new Ogre::TerrainGlobalOptions();
 
   if (this->heightImage.GetWidth() != this->heightImage.GetHeight() ||

gazebo/rendering/Scene.cc

       {
         try
         {
-          this->terrain = new Heightmap(shared_from_this());
-          this->terrain->LoadFromMsg(_msg);
+          if (!this->terrain)
+          {
+            this->terrain = new Heightmap(shared_from_this());
+            this->terrain->LoadFromMsg(_msg);
+          }
+          else
+            gzerr << "Only one Heightmap can be created per Scene\n";
         } catch(...)
         {
           return false;

gazebo/rendering/Visual.cc

   }
   catch(Ogre::Exception &e)
   {
-    gzerr << "Unable to insert mesh[" << e.getDescription() << std::endl;
+    gzerr << "Unable to insert mesh[" << e.getDescription() << "]" << std::endl;
   }
 }
 

gazebo/sdf/1.3/joint.sdf

       <element name="upper" type="double" default="1e16" required="1">
         <description>An attribute specifying the upper joint limit (radians for revolute joints, meters for prismatic joints). Omit if joint is continuous.</description>
       </element>
-      <element name="effort" type="double" default="0" required="0">
-        <description>(not implemented) An attribute for enforcing the maximum joint effort.</description>
+      <element name="effort" type="double" default="-1" required="0">
+        <description>An attribute for enforcing the maximum joint effort applied by Joint::SetForce.  Limit is not enforced if value is negative.</description>
       </element>
-      <element name="velocity" type="double" default="0" required="0">
+      <element name="velocity" type="double" default="-1" required="0">
         <description>(not implemented) An attribute for enforcing the maximum joint velocity.</description>
       </element>
     </element> <!-- End Limit -->
       <element name="upper" type="double" default="1e16" required="0">
         <description>An attribute specifying the upper joint limit (radians for revolute joints, meters for prismatic joints). Omit if joint is continuous.</description>
       </element>
-      <element name="effort" type="double" default="0" required="0">
-        <description>(not implemented) An attribute for enforcing the maximum joint effort.</description>
+      <element name="effort" type="double" default="-1" required="0">
+        <description>An attribute for enforcing the maximum joint effort applied by Joint::SetForce.  Limit is not enforced if value is negative.</description>
       </element>
-      <element name="velocity" type="double" default="0" required="0">
+      <element name="velocity" type="double" default="-1" required="0">
         <description>(not implemented) An attribute for enforcing the maximum joint velocity.</description>
       </element>
     </element> <!-- End Limit -->

gazebo/sdf/interface/parser_urdf.cc

           if ((*ge)->isDampingFactor)
           {
             /// @todo separate linear and angular velocity decay
-            this->AddKeyValue(_elem, "linear",
+            this->AddKeyValue(velocityDecay, "linear",
                         this->Values2str(1, &(*ge)->dampingFactor));
-            this->AddKeyValue(_elem, "angular",
+            this->AddKeyValue(velocityDecay, "angular",
                         this->Values2str(1, &(*ge)->dampingFactor));
           }
           _elem->LinkEndChild(velocityDecay);
   // transform to gazebo::math::Pose then call TransformToParentFrame
   gazebo::math::Pose p1 = URDF2Gazebo::CopyPose(_transformInLinkFrame);
   gazebo::math::Pose p2 = URDF2Gazebo::CopyPose(_parentToLinkTransform);
-  return URDF2Gazebo::CopyPose(TransformToParentFrame(p1, p2));
+  return URDF2Gazebo::CopyPose(this->TransformToParentFrame(p1, p2));
 }
 
 ////////////////////////////////////////////////////////////////////////////////
 {
   // transform to gazebo::math::Pose then call TransformToParentFrame
   gazebo::math::Pose p2 = URDF2Gazebo::CopyPose(_parentToLinkTransform);
-  return TransformToParentFrame(_transformInLinkFrame, p2);
+  return this->TransformToParentFrame(_transformInLinkFrame, p2);
 }
 
 ////////////////////////////////////////////////////////////////////////////////
     for (std::vector<GazeboExtension*>::iterator ge = ext->second.begin();
          ge != ext->second.end(); ++ge)
     {
-      (*ge)->reductionTransform = TransformToParentFrame(
+      (*ge)->reductionTransform = this->TransformToParentFrame(
         (*ge)->reductionTransform,
         _link->parent_joint->parent_to_joint_origin_transform);
       // for sensor and projector blocks only
                 this->Values2str(1, &_link->parent_joint->limits->lower));
               this->AddKeyValue(jointAxisLimit, "upper",
                 this->Values2str(1, &_link->parent_joint->limits->upper));
+              this->AddKeyValue(jointAxisLimit, "effort",
+                this->Values2str(1, &_link->parent_joint->limits->effort));
+              this->AddKeyValue(jointAxisLimit, "velocity",
+                this->Values2str(1, &_link->parent_joint->limits->velocity));
             }
             else if (_link->parent_joint->type != urdf::Joint::CONTINUOUS)
             {
                 this->Values2str(1, &_link->parent_joint->limits->lower));
               this->AddKeyValue(jointAxisLimit, "upper",
                 this->Values2str(1, &_link->parent_joint->limits->upper));
+              this->AddKeyValue(jointAxisLimit, "effort",
+                this->Values2str(1, &_link->parent_joint->limits->effort));
+              this->AddKeyValue(jointAxisLimit, "velocity",
+                this->Values2str(1, &_link->parent_joint->limits->velocity));
             }
           }
         }
     visualsIt = _link->visual_groups.begin();
     visualsIt != _link->visual_groups.end(); ++visualsIt)
   {
-    /// @todo: extend to different groups,
-    /// only work with default meshes right now.
-    if (visualsIt->first == "default")
-    {
-      std::string lumpGroupName = std::string("lump::")+_link->name;
-      // gzdbg << "adding modified lump group name [" << lumpGroupName
-      //       << "] to link [" << _link->getParent()->name << "]\n.";
-      for (std::vector<UrdfVisualPtr>::iterator
-        visualIt = visualsIt->second->begin();
-        visualIt != visualsIt->second->end(); ++visualIt)
-      {
-        // transform visual origin from _link frame to
-        // parent link frame before adding to parent
-        (*visualIt)->origin = TransformToParentFrame((*visualIt)->origin,
-          _link->parent_joint->parent_to_joint_origin_transform);
-        // add the modified visual to parent
-        this->ReduceVisualToParent(_link->getParent(), lumpGroupName,
-          *visualIt);
-      }
-    }
-    else if (visualsIt->first.find(std::string("lump::")) == 0)
+    if (visualsIt->first.find(std::string("lump::")) == 0)
     {
       // it's a previously lumped mesh, re-lump under same _groupName
       std::string lumpGroupName = visualsIt->first;
       {
         // transform visual origin from _link frame to parent link
         // frame before adding to parent
-        (*visualIt)->origin = TransformToParentFrame((*visualIt)->origin,
+        (*visualIt)->origin = this->TransformToParentFrame((*visualIt)->origin,
+          _link->parent_joint->parent_to_joint_origin_transform);
+        // add the modified visual to parent
+        this->ReduceVisualToParent(_link->getParent(), lumpGroupName,
+          *visualIt);
+      }
+    }
+    else
+    {
+      // default and any other groups meshes
+      std::string lumpGroupName = std::string("lump::")+_link->name;
+      // gzdbg << "adding modified lump group name [" << lumpGroupName
+      //       << "] to link [" << _link->getParent()->name << "]\n.";
+      for (std::vector<UrdfVisualPtr>::iterator
+        visualIt = visualsIt->second->begin();
+        visualIt != visualsIt->second->end(); ++visualIt)
+      {
+        // transform visual origin from _link frame to
+        // parent link frame before adding to parent
+        (*visualIt)->origin = this->TransformToParentFrame((*visualIt)->origin,
           _link->parent_joint->parent_to_joint_origin_transform);
         // add the modified visual to parent
         this->ReduceVisualToParent(_link->getParent(), lumpGroupName,
       collisionsIt = _link->collision_groups.begin();
       collisionsIt != _link->collision_groups.end(); ++collisionsIt)
     {
-      if (collisionsIt->first == "default")
-      {
-        // if it's a "default" mesh, it will be added under "lump::"+_link name
-        std::string lumpGroupName = std::string("lump::")+_link->name;
-        // gzdbg << "lumping collision [" << collisionsIt->first
-        //       << "] for link [" << _link->name
-        //       << "] to parent [" << _link->getParent()->name
-        //       << "] with group name [" << lumpGroupName << "]\n";
-        for (std::vector<UrdfCollisionPtr>::iterator
-          collisionIt = collisionsIt->second->begin();
-          collisionIt != collisionsIt->second->end(); ++collisionIt)
-        {
-          // transform collision origin from _link frame to
-          // parent link frame before adding to parent
-          (*collisionIt)->origin = TransformToParentFrame(
-            (*collisionIt)->origin,
-            _link->parent_joint->parent_to_joint_origin_transform);
-
-          // add the modified collision to parent
-          this->ReduceCollisionToParent(_link->getParent(), lumpGroupName,
-            *collisionIt);
-        }
-      }
-      else if (collisionsIt->first.find(std::string("lump::")) == 0)
+      if (collisionsIt->first.find(std::string("lump::")) == 0)
       {
         // if it's a previously lumped mesh, relump under same _groupName
         std::string lumpGroupName = collisionsIt->first;
         {
           // transform collision origin from _link frame to
           // parent link frame before adding to parent
-          (*collisionIt)->origin = TransformToParentFrame(
+          (*collisionIt)->origin = this->TransformToParentFrame(
             (*collisionIt)->origin,
             _link->parent_joint->parent_to_joint_origin_transform);
           // add the modified collision to parent
             *collisionIt);
         }
       }
+      else
+      {
+        // default and any other group meshes
+        std::string lumpGroupName = std::string("lump::")+_link->name;
+        // gzdbg << "lumping collision [" << collisionsIt->first
+        //       << "] for link [" << _link->name
+        //       << "] to parent [" << _link->getParent()->name
+        //       << "] with group name [" << lumpGroupName << "]\n";
+        for (std::vector<UrdfCollisionPtr>::iterator
+          collisionIt = collisionsIt->second->begin();
+          collisionIt != collisionsIt->second->end(); ++collisionIt)
+        {
+          // transform collision origin from _link frame to
+          // parent link frame before adding to parent
+          (*collisionIt)->origin = this->TransformToParentFrame(
+            (*collisionIt)->origin,
+            _link->parent_joint->parent_to_joint_origin_transform);
+
+          // add the modified collision to parent
+          this->ReduceCollisionToParent(_link->getParent(), lumpGroupName,
+            *collisionIt);
+        }
+      }
     }
     // this->PrintCollisionGroups(_link->getParent());
 }
         {
           jointAnchorTransform = jointAnchorTransform * jointAnchorTransform;
           parentJoint->parent_to_joint_origin_transform =
-            TransformToParentFrame(
+            this->TransformToParentFrame(
             parentJoint->parent_to_joint_origin_transform,
             newParentLink->parent_joint->parent_to_joint_origin_transform);
           newParentLink = newParentLink->getParent();

gazebo/sensors/CMakeLists.txt

 
 set (gtest_sources
   RaySensor_TEST.cc
+  ImuSensor_TEST.cc
 )
 
 if (${VALID_DRI_DISPLAY})

gazebo/sensors/ContactSensor.hh

       public: unsigned int GetCollisionContactCount(
                   const std::string &_collisionName) const;
 
-      /// \brief Get all the contacts
-      /// \return Message that contains all the contact information
+      /// \brief Get all the contacts for the ContactSensor
+      /// \return Message that contains contact information between collision
+      /// pairs.
+      ///
+      /// During ODEPhysics::UpdateCollisions, all collision pairs in the
+      /// world are pushed into a buffer within ContactManager.
+      /// Subsequently, World::Update invokes ContactManager::PublishContacts
+      /// to publish all contacts generated within a timestep onto
+      /// Gazebo topic ~/physics/contacts.
+      ///
+      /// Each ContactSensor subscribes to the Gazebo ~/physics/contacts topic,
+      /// retrieves all contact pairs in a time step and filters them wthin
+      /// ContactSensor::OnContacts against <collision> body name
+      /// specified by the ContactSensor SDF.
+      /// All collision pairs between ContactSensor <collision> body and
+      /// other bodies in the world are stored in an array inside
+      /// contacts.proto.
+      ///
+      /// Within each element of the contact.proto array inside contacts.proto,
+      /// list of collisions between collision bodies
+      /// (collision1 and collision 2) are stored in an array of
+      /// elements, (position, normal, depth, wrench).  A timestamp has also
+      /// been added (time).  Details are described below:
+      ///
+      ///    \li string collision1  name of the first collision object.
+      ///    \li string collision2  name of the second collision object.
+      ///    \li Vector3d position  position of the contact joint in
+      ///                           inertial frame.
+      ///    \li Vector3d normal    normal of the contact joint in
+      ///                           inertial frame.
+      ///    \li double depth       intersection (penetration)
+      ///                           depth of two collision bodies.
+      ///    \li JointWrench wrench Forces and torques acting on both collision
+      ///                           bodies.  See joint_wrench.proto for details.
+      ///                           The forces and torques are applied at the
+      ///                           CG of perspective links for each collision
+      ///                           body, specified in the inertial frame.
+      ///    \li Time time          time at which this contact happened.
       public: msgs::Contacts GetContacts() const;
 
       /// \brief Gets contacts of a collision

gazebo/sensors/ImuSensor.cc

 
 #include "gazebo/physics/Link.hh"
 #include "gazebo/physics/World.hh"
+#include "gazebo/physics/PhysicsEngine.hh"
 
 #include "gazebo/sensors/SensorFactory.hh"
 #include "gazebo/sensors/ImuSensor.hh"
 
 //////////////////////////////////////////////////
 ImuSensor::ImuSensor()
-    : Sensor(sensors::OTHER)
+  : Sensor(sensors::OTHER)
 {
 }
 
 {
   Sensor::Load(_worldName, _sdf);
 
-  this->sdf->PrintValues("  ");
-
   if (this->sdf->HasElement("imu") &&
       this->sdf->GetElement("imu")->HasElement("topic") &&
       this->sdf->GetElement("imu")->GetValueString("topic")
     gzthrow("IMU has invalid paret[" + this->parentName +
             "]. Must be a link\n");
   }
+  this->referencePose = this->pose + this->parentEntity->GetWorldPose();
+  this->lastLinearVel = this->referencePose.rot.RotateVector(
+    this->parentEntity->GetWorldLinearVel());
 }
 
 //////////////////////////////////////////////////
 void ImuSensor::Init()
 {
+  Sensor::Init();
 }
 
 //////////////////////////////////////////////////
 }
 
 //////////////////////////////////////////////////
+math::Quaternion ImuSensor::GetOrientation() const
+{
+  return msgs::Convert(this->imuMsg.orientation());
+}
+
+//////////////////////////////////////////////////
+void ImuSensor::SetReferencePose()
+{
+  this->referencePose = this->pose + this->parentEntity->GetWorldPose();
+}
+
+//////////////////////////////////////////////////
 void ImuSensor::UpdateImpl(bool /*_force*/)
 {
+  double dt = (this->world->GetSimTime() - this->lastMeasurementTime).Double();
+
   this->lastMeasurementTime = this->world->GetSimTime();
 
   this->imuMsg.set_entity_name(this->parentName);
   // Set the time stamp
   msgs::Set(this->imuMsg.mutable_stamp(), this->world->GetSimTime());
 
+  math::Pose parentEntityPose = this->parentEntity->GetWorldPose();
+  math::Pose imuPose = this->pose + parentEntityPose;
+
   // Set the IMU orientation
   msgs::Set(this->imuMsg.mutable_orientation(),
-            this->parentEntity->GetWorldPose().rot);
+            imuPose.rot * this->referencePose.rot.GetInverse());
 
   // Set the IMU angular velocity
   msgs::Set(this->imuMsg.mutable_angular_velocity(),
-            this->parentEntity->GetWorldAngularVel());
+            imuPose.rot.GetInverse().RotateVector(
+            this->parentEntity->GetWorldAngularVel()));
 
-  // Set the IMU linear acceleration
-  msgs::Set(this->imuMsg.mutable_linear_acceleration(),
-            this->parentEntity->GetWorldLinearAccel());
+  // get linear velocity in world frame
+  math::Vector3 imuWorldLinearVel =
+    this->parentEntity->GetWorldLinearVel(this->pose.pos);
+
+  // Compute and set the IMU linear acceleration
+  if (dt > 0.0)
+  {
+    this->linearAcc = imuPose.rot.GetInverse().RotateVector(
+      (imuWorldLinearVel - this->lastLinearVel) / dt);
+    this->lastLinearVel = imuWorldLinearVel;
+  }
+
+  // Add contribution from gravity
+  this->gravity = this->world->GetPhysicsEngine()->GetGravity();
+  this->linearAcc += imuPose.rot.GetInverse().RotateVector(this->gravity);
+
+  msgs::Set(this->imuMsg.mutable_linear_acceleration(), this->linearAcc);
 
   if (this->pub)
     this->pub->Publish(this->imuMsg);

gazebo/sensors/ImuSensor.hh

       /// \return Angular velocity.
       public: math::Vector3 GetAngularVelocity() const;
 
-      /// \brief Returns the linear acceleration.
+      /// \brief Returns the imu linear acceleration
       /// \return Linear acceleration.
       public: math::Vector3 GetLinearAcceleration() const;
 
+      /// \brief get orientation of the IMU relative to the reference pose
+      /// \return returns the orientation quaternion of the IMU relative to
+      /// the imu reference pose.
+      public: math::Quaternion GetOrientation() const;
+
+      /// \brief Sets the current pose as the IMU reference pose
+      public: void SetReferencePose();
+
+      /// \brief Imu reference pose
+      private: math::Pose referencePose;
+
+      /// \brief Save previous imu linear velocity for computing acceleration.
+      private: math::Vector3 lastLinearVel;
+
+      /// \brief Imu linear acceleration
+      private: math::Vector3 linearAcc;
+
+      /// \brief store gravity vector to be added to the imu output.
+      private: math::Vector3 gravity;
+
       private: transport::PublisherPtr pub;
       private: physics::LinkPtr parentEntity;
       private: msgs::IMU imuMsg;

gazebo/sensors/ImuSensor_TEST.cc

+/*
+ * Copyright 2013 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include <sys/time.h>
+#include <gtest/gtest.h>
+
+#include "test/ServerFixture.hh"
+#include "gazebo/sensors/ImuSensor.hh"
+
+using namespace gazebo;
+class ImuSensor_TEST : public ServerFixture
+{
+};
+
+static std::string imuSensorString =
+"<sdf version='1.3'>"
+"  <sensor name='imu' type='imu'>"
+"    <always_on>1</always_on>"
+"    <update_rate>20.000000</update_rate>"
+"    <imu>"
+"      <topic>/test_imu</topic>"
+"    </imu>"
+"  </sensor>"
+"</sdf>";
+
+TEST_F(ImuSensor_TEST, BasicImuSensorCheck)
+{
+  Load("worlds/empty.world");
+  sensors::SensorManager *mgr = sensors::SensorManager::Instance();
+
+  sdf::ElementPtr sdf(new sdf::Element);
+  sdf::initFile("sensor.sdf", sdf);
+  sdf::readString(imuSensorString, sdf);
+
+  // Create the IMU sensor
+  std::string sensorName = mgr->CreateSensor(sdf, "default",
+      "ground_plane::link");
+
+  // Make sure the returned sensor name is correct
+  EXPECT_EQ(sensorName, std::string("default::ground_plane::link::imu"));
+
+  // Update the sensor manager so that it can process new sensors.
+  mgr->Update();
+
+  // Get a pointer to the IMU sensor
+  sensors::ImuSensorPtr sensor = boost::shared_dynamic_cast<sensors::ImuSensor>
+    (mgr->GetSensor(sensorName));
+
+  // Make sure the above dynamic cast worked.
+  EXPECT_TRUE(sensor != NULL);
+
+  EXPECT_EQ(sensor->GetAngularVelocity(), math::Vector3::Zero);
+  EXPECT_EQ(sensor->GetLinearAcceleration(), math::Vector3::Zero);
+  EXPECT_EQ(sensor->GetOrientation(), math::Quaternion(0, 0, 0, 0));
+}
+
+
+/////////////////////////////////////////////////
+int main(int argc, char **argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

gazebo/sensors/SensorTypes.hh

     class CameraSensor;
     class DepthCameraSensor;
     class ContactSensor;
+    class ImuSensor;
     class GpuRaySensor;
     class RFIDSensor;
     class RFIDTag;
     /// \brief Shared pointer to ContactSensor
     typedef boost::shared_ptr<ContactSensor> ContactSensorPtr;
 
+    /// \def ImuSensorPtr
+    /// \brief Shared pointer to ImuSensor
+    typedef boost::shared_ptr<ImuSensor> ImuSensorPtr;
+
     /// \def GpuRaySensorPtr
     /// \brief Shared pointer to GpuRaySensor
     typedef boost::shared_ptr<GpuRaySensor> GpuRaySensorPtr;
     /// \brief Vector of ContactSensor shared pointers
     typedef std::vector<ContactSensorPtr> ContactSensor_V;
 
+    /// \def ImuSensor_V
+    /// \brief Vector of ImuSensor shared pointers
+    typedef std::vector<ImuSensorPtr> ImuSensor_V;
+
     /// \def GpuRaySensor_V
     /// \brief Vector of GpuRaySensor shared pointers
     typedef std::vector<GpuRaySensorPtr> GpuRaySensor_V;

gazebo/transport/Connection.cc

   boost::asio::ip::tcp::resolver resolver(iomanager->GetIO());
   boost::asio::ip::tcp::resolver::query query(host, service,
       boost::asio::ip::resolver_query_base::numeric_service);
-  boost::asio::ip::tcp::resolver::iterator endpoint_iter;
+  boost::asio::ip::tcp::resolver::iterator endpointIter;
+
   try
   {
-    endpoint_iter = resolver.resolve(query);
+    endpointIter = resolver.resolve(query);
+
+    // Find the first valid IPv4 address
+    for (; endpointIter != end &&
+           !(*endpointIter).endpoint().address().is_v4(); ++endpointIter)
+    {
+    }
+
+    // Make sure we didn't run off the end of the list.
+    if (endpointIter == end)
+    {
+      gzerr << "Unable to resolve uri[" << _host << ":" << _port << "]\n";
+      return false;
+    }
   }
   catch(...)
   {
   this->connectError = false;
   this->remoteURI.clear();
 
-  {
-    // Use async connect so that we can use a custom timeout. This is useful
-    // when trying to detect network errors.
-    this->socket->async_connect(*endpoint_iter++,
-        boost::bind(&Connection::OnConnect, this,
-          boost::asio::placeholders::error, endpoint_iter));
-  }
+  // Use async connect so that we can use a custom timeout. This is useful
+  // when trying to detect network errors.
+  this->socket->async_connect(*endpointIter++,
+      boost::bind(&Connection::OnConnect, this,
+        boost::asio::placeholders::error, endpointIter));
 
   // Wait for at most 2 seconds for a connection to be established.
   // The connectionCondition notification occurs in ::OnConnect.
     std::size_t written = 0;
     written = boost::asio::write(*this->socket, buffer->data());
     if (written != buffer->size())
-    gzerr << "Didn't write all the data\n";
+      gzerr << "Didn't write all the data\n";
 
     delete buffer;
     }
   this->readMutex->lock();
 
   // First read the header
-  {
-    this->socket->read_some(boost::asio::buffer(header), error);
-  }
+  this->socket->read_some(boost::asio::buffer(header), error);
 
   if (error)
   {
         continue;
 
       int family = ifa->ifa_addr->sa_family;
-      if (family == AF_INET || family == AF_INET6)
+      // \todo We currently don't handle AF_INET6 addresses. So I commented
+      // out the below line, and removed AF_INET6 for the if clause.
+      // if (family == AF_INET || family == AF_INET6)
+      if (family == AF_INET)
       {
         int s = getnameinfo(ifa->ifa_addr,
             (family == AF_INET) ? sizeof(struct sockaddr_in) :

test/ServerFixture.hh

                  ASSERT_NO_THROW(this->server->LoadFile(_worldFilename));
                ASSERT_NO_THROW(this->server->Init());
 
-               rendering::create_scene(
-                   gazebo::physics::get_world()->GetName(), false);
+               if (!rendering::get_scene(
+                     gazebo::physics::get_world()->GetName()))
+               {
+                 rendering::create_scene(
+                     gazebo::physics::get_world()->GetName(), false);
+               }
 
                this->SetPause(_paused);
 
                EXPECT_LT(i, 50);
              }
 
+  protected: void SpawnRaySensor(const std::string &_modelName,
+                 const std::string &_raySensorName,
+                 const math::Vector3 &_pos, const math::Vector3 &_rpy,
+                 double _hMinAngle = -2.0, double _hMaxAngle = 2.0,
+                 double _minRange = 0.08, double _maxRange = 10,
+                 double _rangeResolution = 0.01, unsigned int _samples = 640)
+             {
+               msgs::Factory msg;
+               std::ostringstream newModelStr;
+
+               newModelStr << "<sdf version='" << SDF_VERSION << "'>"
+                 << "<model name ='" << _modelName << "'>"
+                 << "<static>true</static>"
+                 << "<pose>" << _pos.x << " "
+                             << _pos.y << " "
+                             << _pos.z << " "
+                             << _rpy.x << " "
+                             << _rpy.y << " "
+                             << _rpy.z << "</pose>"
+                 << "<link name ='body'>"
+                 << "<collision name='parent_collision'>"
+                 << "  <pose>0 0 0.0205 0 0 0</pose>"
+                 << "  <geometry>"
+                 << "    <cylinder>"
+                 << "      <radius>0.021</radius>"
+                 << "      <length>0.029</length>"
+                 << "    </cylinder>"
+                 << "  </geometry>"
+                 << "</collision>"
+                 << "  <sensor name ='" << _raySensorName << "' type ='ray'>"
+                 << "    <ray>"
+                 << "      <scan>"
+                 << "        <horizontal>"
+                 << "          <samples>" << _samples << "</samples>"
+                 << "          <resolution> 1 </resolution>"
+                 << "          <min_angle>" << _hMinAngle << "</min_angle>"
+                 << "          <max_angle>" << _hMaxAngle << "</max_angle>"
+                 << "        </horizontal>"
+                 << "      </scan>"
+                 << "      <range>"
+                 << "        <min>" << _minRange << "</min>"
+                 << "        <max>" << _maxRange << "</max>"
+                 << "        <resolution>" << _rangeResolution <<"</resolution>"
+                 << "      </range>"
+                 << "    </ray>"
+                 << "  </sensor>"
+                 << "</link>"
+                 << "</model>"
+                 << "</sdf>";
+
+               msg.set_sdf(newModelStr.str());
+               this->factoryPub->Publish(msg);
+
+               int i = 0;
+               // Wait for the entity to spawn
+               while (!this->HasEntity(_modelName) && i < 50)
+               {
+                 common::Time::MSleep(20);
+                 ++i;
+               }
+               EXPECT_LT(i, 50);
+             }
+
   protected: void SpawnCylinder(const std::string &_name,
                  const math::Vector3 &_pos, const math::Vector3 &_rpy)
              {
                  common::Time::MSleep(10);
              }
 
+  protected: void SpawnTrimesh(const std::string &_name,
+                 const std::string &_modelPath, const math::Vector3 &_scale,
+                 const math::Vector3 &_pos, const math::Vector3 &_rpy)
+             {
+               msgs::Factory msg;
+               std::ostringstream newModelStr;
+
+               newModelStr << "<sdf version='" << SDF_VERSION << "'>"
+                 << "<model name ='" << _name << "'>"
+                 << "<pose>" << _pos.x << " "
+                             << _pos.y << " "
+                             << _pos.z << " "
+                             << _rpy.x << " "
+                             << _rpy.y << " "
+                             << _rpy.z << "</pose>"
+                 << "<link name ='body'>"
+                 << "  <collision name ='geom'>"
+                 << "    <geometry>"
+                 << "      <mesh>"
+                 << "        <uri>" << _modelPath << "</uri>"
+                 << "        <scale>" << _scale << "</scale>"
+                 << "      </mesh>"
+                 << "    </geometry>"
+                 << "  </collision>"
+                 << "  <visual name ='visual'>"
+                 << "    <geometry>"
+                 << "      <mesh><uri>" << _modelPath << "</uri></mesh>"
+                 << "    </geometry>"
+                 << "  </visual>"
+                 << "</link>"
+                 << "</model>"
+                 << "</sdf>";
+
+               msg.set_sdf(newModelStr.str());
+               this->factoryPub->Publish(msg);
+
+               // Wait for the entity to spawn
+               while (!this->HasEntity(_name))
+                 common::Time::MSleep(10);
+             }
+
   protected: void SpawnEmptyLink(const std::string &_name,
                  const math::Vector3 &_pos,
                  const math::Vector3 &_rpy)

test/regression/CMakeLists.txt

 include_directories (
-  ${PROJECT_SOURCE_DIR}/gazebo 
+  ${PROJECT_SOURCE_DIR}/gazebo
   ${PROJECT_BINARY_DIR}/gazebo
   ${ODE_INCLUDE_DIRS}
   ${OPENGL_INCLUDE_DIR}
   ${PROTOBUF_INCLUDE_DIR}
 )
 
-link_directories(  
+link_directories(
   ${ogre_library_dirs}
-  ${Boost_LIBRARY_DIRS} 
+  ${Boost_LIBRARY_DIRS}
   ${ODE_LIBRARY_DIRS}
 )
 
   link_directories ( ${BULLET_LIBRARY_DIRS} )
 endif()
 
-include_directories(${PROJECT_SOURCE_DIR}/test/gtest/include)
-
-set (gtest_sources
+set(tests
   bandwidth.cc
-  camera_sensor.cc
   factory.cc
   file_handling.cc
-  heightmap.cc
   laser.cc
   msgs.cc
   physics.cc
   pioneer2dx.cc
+  transport.cc
+  server_fixture.cc
+  speed.cc
+  )
+
+set (GZ_BUILD_TESTS_EXTRA_EXE_SRCS
+    "${PROJECT_SOURCE_DIR}/gazebo/Server.cc;${PROJECT_SOURCE_DIR}/gazebo/Master.cc;${PROJECT_SOURCE_DIR}/gazebo/gazebo.cc")
+gz_build_tests(${tests})
+
+set(display_tests
+)
+
+# Build the display tests (need extra sources to compile)
+set (GZ_BUILD_TESTS_EXTRA_EXE_SRCS
+    "${PROJECT_SOURCE_DIR}/gazebo/Server.cc;${PROJECT_SOURCE_DIR}/gazebo/Master.cc;${PROJECT_SOURCE_DIR}/gazebo/gazebo.cc")
+gz_build_display_tests(${display_tests})
+
+set(dri_tests
+  camera_sensor.cc
+  heightmap.cc
   pr2.cc
   projector.cc
-  server_fixture.cc
-  speed.cc
   speed_pr2.cc
-  transport.cc)
+)
 
-set (plugins)
-
-# Build plugins
-foreach (src ${plugins})
-  add_library(${src} SHARED ${src}.cc)
-  target_link_libraries(${src} libgazebo gazebo_sensors)
-endforeach (src ${plugins})
-
-# Build tests
-foreach(GTEST_SOURCE_file ${gtest_sources})
-  string(REGEX REPLACE ".cc" "" BINARY_NAME ${GTEST_SOURCE_file})
-  add_executable(${BINARY_NAME} ${GTEST_SOURCE_file} ${PROJECT_SOURCE_DIR}/gazebo/Server.cc ${PROJECT_SOURCE_DIR}/gazebo/Master.cc ${PROJECT_SOURCE_DIR}/gazebo/gazebo.cc)
-
-target_link_libraries(${BINARY_NAME}
-  ${GTEST_LIBRARY} 
-  ${GTEST_MAIN_LIBRARY} 
-    gazebo_common
-    gazebo_sdf_interface
-    gazebo_transport
-    gazebo_physics
-    gazebo_sensors
-    gazebo_rendering
-    gazebo_msgs
-    pthread
-    ${tinyxml_libraries}
-    )
-  add_test(${BINARY_NAME} ${CMAKE_CURRENT_BINARY_DIR}/${BINARY_NAME} --gtest_output=xml:${CMAKE_BINARY_DIR}/test_results/${BINARY_NAME}.xml)
-  set_tests_properties(${BINARY_NAME} PROPERTIES TIMEOUT 240)
-  # Check that the test produced a result and create a failure if it didn't.
-  # Guards against crashed and timed out tests.
-  add_test(check_${BINARY_NAME} ${PROJECT_SOURCE_DIR}/tools/check_test_ran.py ${CMAKE_BINARY_DIR}/test_results/${BINARY_NAME}.xml)
-endforeach()
+set (GZ_BUILD_TESTS_EXTRA_EXE_SRCS
+    "${PROJECT_SOURCE_DIR}/gazebo/Server.cc;${PROJECT_SOURCE_DIR}/gazebo/Master.cc;${PROJECT_SOURCE_DIR}/gazebo/gazebo.cc")
+gz_build_dri_tests(${dri_tests})

test/regression/heightmap.cc

 {
 };
 
-std::string heightmapModel =
-"<sdf version='1.3'>"
-"<model name='heightmap'>"
-"      <static>true</static>"
-"      <link name='link'>"
-"        <collision name='collision'>"
-"          <geometry>"
-"            <heightmap>"
-"              <uri>file://media/materials/textures/heightmap_bowl.png</uri>"
-"              <size>129 129 10</size>"
-"              <pos>0 0 0</pos>"
-"            </heightmap>"
-"          </geometry>"
-"        </collision>"
-"        <visual name='visual'>"
-"          <geometry>"
-"            <heightmap>"
-"              <texture>"
-"                <diffuse>file://media/materials/textures/"
-"dirt_diffusespecular.png</diffuse>"
-"                <normal>file://media/materials/textures/"
-"flat_normal.png</normal>"
-"                <size>50</size>"