Commits

John Hsu committed 323c4c5 Merge

merging from default

Comments (0)

Files changed (121)

 57f6dd87296d23c72fabdb495670fa77370c8d75 gazebo_1.2.6
 57f6dd87296d23c72fabdb495670fa77370c8d75 gazebo_1.2.6
 f3163222038fd0daff3b16d0f8ee634d53016cdc gazebo_1.2.6
+d212ad872965ca4643589c0f0ae386b92de36a29 gazebo_1.3.0
 02a2ec16375a0825ea76c824f577ab0568bedbc0 gazebo_1.3.0
 02a2ec16375a0825ea76c824f577ab0568bedbc0 gazebo_1.3.0
-d212ad872965ca4643589c0f0ae386b92de36a29 gazebo_1.3.0
 6cefc0062eaf56a3f414d2d2e0cde897be75b840 gazebo-prerelease_1.3.0
 6cefc0062eaf56a3f414d2d2e0cde897be75b840 gazebo-prerelease_1.3.0
 dc47d59da98b573859681e458e0961b0863ebf25 gazebo-prerelease_1.3.0
+ca149ddc5402996433830057cb7b4a605a636383 gazebo-prerelease_1.3.0
 
   configure_file(${CMAKE_SOURCE_DIR}/cmake/setup.sh.in ${PROJECT_BINARY_DIR}/setup.sh @ONLY)
   install(FILES ${CMAKE_CURRENT_BINARY_DIR}/setup.sh DESTINATION ${CMAKE_INSTALL_PREFIX}/share/gazebo-${GAZEBO_VERSION}/)
+  configure_file(${CMAKE_SOURCE_DIR}/cmake/setup-unversioned.sh.in ${PROJECT_BINARY_DIR}/unversioned/setup.sh @ONLY)
+  install(FILES ${CMAKE_CURRENT_BINARY_DIR}/unversioned/setup.sh DESTINATION ${CMAKE_INSTALL_PREFIX}/share/gazebo/)
  
   include_directories(${PROJECT_SOURCE_DIR} ${PROJECT_BINARY_DIR})
   

cmake/setup-unversioned.sh.in

+# This file is intended to be used when you want the latest installed version of
+# Gazebo.
+
+# Find all Gazebo installations with setup.sh files, sort them, and take the
+# last one (which should be the latest, assuming lexicographic sorting and
+# strictly increasing version numbers).
+setup=`find @CMAKE_INSTALL_PREFIX@/share -path @CMAKE_INSTALL_PREFIX@/share/gazebo-'*'/setup.sh | sort | tail -n 1`
+
+if [ -z $setup ]; then
+  echo "ERROR: Couldn't find a Gazebo setup.sh in @CMAKE_INSTALL_PREFIX@/share"
+  false
+elif [ ! -f $setup ]; then
+  echo "ERROR: Setup file $setup doesn't exist"
+  false
+else
+  # Otherwise, looks good.  Source it.
+  . $setup
+fi

deps/opende/src/quickstep.cpp

       printf ("velocity error = %10.6e\n",error);
     }
   }
-
 #endif
 
-
-
   {
     // update the position and orientation from the new linear/angular velocity
     // (over the given timestep)

gazebo/gui/MainWindow.cc

 /////////////////////////////////////////////////
 void MainWindow::Load()
 {
-  this->guiSub = this->node->Subscribe("~/gui", &MainWindow::OnGUI, this);
+  this->guiSub = this->node->Subscribe("~/gui", &MainWindow::OnGUI, this, true);
 }
 
 /////////////////////////////////////////////////
 }
 
 /////////////////////////////////////////////////
-void MainWindow::Save()
-{
-  msgs::ServerControl msg;
-  msg.set_save_world_name(get_world());
-  this->serverControlPub->Publish(msg);
-}
-
-/////////////////////////////////////////////////
 void MainWindow::SaveAs()
 {
   std::string filename = QFileDialog::getSaveFileName(this,
       tr("Save World"), QString(),
       tr("SDF Files (*.xml *.sdf *.world)")).toStdString();
 
-  if (!filename.empty())
+  // Return if the user has canceled.
+  if (filename.empty())
+    return;
+
+  g_saveAct->setEnabled(true);
+  this->saveFilename = filename;
+  this->Save();
+}
+
+/////////////////////////////////////////////////
+void MainWindow::Save()
+{
+  // Get the latest world in SDF.
+  boost::shared_ptr<msgs::Response> response =
+    transport::request(get_world(), "world_sdf");
+
+  msgs::GzString msg;
+
+  // Make sure the response is correct
+  if (response->response() != "error" && response->type() == msg.GetTypeName())
   {
-    msgs::ServerControl msg;
-    msg.set_save_world_name(get_world());
-    msg.set_save_filename(filename);
-    this->serverControlPub->Publish(msg);
+    // Parse the response
+    msg.ParseFromString(response->serialized_data());
+
+    // Open the file
+    std::ofstream out(this->saveFilename.c_str(), std::ios::out);
+
+    if (!out)
+    {
+      QMessageBox msgBox;
+      std::string str = "Unable to open file: " + this->saveFilename + "\n";
+      str += "Check file permissions.";
+      msgBox.setText(str.c_str());
+      msgBox.exec();
+    }
+    else
+      out << msg.data();
+
+    out.close();
+  }
+  else
+  {
+    QMessageBox msgBox;
+    msgBox.setText("Unable to save world.\n"
+                   "Unable to retrieve SDF world description from server.");
+    msgBox.exec();
   }
 }
 
   connect(g_importAct, SIGNAL(triggered()), this, SLOT(Import()));
   */
 
-  /*
   g_saveAct = new QAction(tr("&Save World"), this);
   g_saveAct->setShortcut(tr("Ctrl+S"));
   g_saveAct->setStatusTip(tr("Save world"));
+  g_saveAct->setEnabled(false);
   connect(g_saveAct, SIGNAL(triggered()), this, SLOT(Save()));
-  */
 
   g_saveAsAct = new QAction(tr("Save World &As"), this);
   g_saveAsAct->setShortcut(tr("Ctrl+Shift+S"));
   // this->fileMenu->addAction(g_openAct);
   // this->fileMenu->addAction(g_importAct);
   // this->fileMenu->addAction(g_newAct);
-  // this->fileMenu->addAction(g_saveAct);
+  this->fileMenu->addAction(g_saveAct);
   this->fileMenu->addAction(g_saveAsAct);
   this->fileMenu->addSeparator();
   this->fileMenu->addAction(g_quitAct);

gazebo/gui/MainWindow.hh

       // private: QTreeWidget *treeWidget;
       private: QTabWidget *tabWidget;
       private: QMenuBar *menuBar;
+
+      /// \brief The filename set via "Save As". This filename is used by
+      /// the "Save" feature.
+      private: std::string saveFilename;
     };
 
     class TreeViewDelegate: public QItemDelegate

gazebo/gui/images/copy_object.png

Added
New image

gazebo/gui/images/paste_object.png

Added
New image

gazebo/gui/images/rotate_object.png

Added
New image

gazebo/gui/images/scale_object.png

Added
New image

gazebo/gui/resources.qrc

   <file>images/rulers.png</file>
   <file>images/snap_to_grid.png</file>
   <file>images/undo.png</file>
+  <file>images/copy_object.png</file>
+  <file>images/paste_object.png</file>
+  <file>images/rotate_object.png</file>
+  <file>images/scale_object.png</file>
 </qresource>
 </RCC>

gazebo/gui/style.qss

   color: #f58113;
 }
 
+QMenu::item:disabled
+{
+  color: #5f5f5f;
+}
+
 QToolBar
 {
   padding: 0px;

gazebo/math/Angle.cc

 }
 
 //////////////////////////////////////////////////
-double Angle::GetAsRadian() const
-{
-  return this->value;
-}
-
-//////////////////////////////////////////////////
 double Angle::Radian() const
 {
   return this->value;
 }
 
 //////////////////////////////////////////////////
-double Angle::GetAsDegree() const
-{
-  return this->value * 180.0 / M_PI;
-}
-
-//////////////////////////////////////////////////
 double Angle::Degree() const
 {
   return this->value * 180.0 / M_PI;

gazebo/math/Angle.hh

 
     /// \brief Get the angle in radians
     /// \return double containing the angle's radian value
-    public: double GetAsRadian() const __attribute__((deprecated));
-
-    /// \brief Get the angle in radians
-    /// \return double containing the angle's radian value
     public: double Radian() const;
 
     /// \brief Get the angle in degrees
     /// \return double containing the angle's degree value
-    public: double GetAsDegree() const __attribute__((deprecated));
-
-    /// \brief Get the angle in degrees
-    /// \return double containing the angle's degree value
     public: double Degree() const;
 
     /// \brief Normalize the angle in the range -Pi to Pi

gazebo/math/Helpers.hh

 #define _GAZEBO_MATH_FUNCTIONS_HH_
 
 #include <boost/math/special_functions/fpclassify.hpp>
+#include <boost/math/special_functions/round.hpp>
 #include <algorithm>
 #include <cmath>
 #include <limits>
     /// \brief Returns the representation of a quiet not a number (NAN)
     static const int NAN_I = std::numeric_limits<int>::quiet_NaN();
 
-    /// \brief simple clamping function
+    /// \brief Simple clamping function
     /// \param[in] _v value
     /// \param[in] _min minimum
     /// \param[in] _max maximum
     template<typename T>
     inline T precision(const T &_a, const unsigned int &_precision)
     {
-      return round(_a * pow(10, _precision)) / pow(10, _precision);
+      return boost::math::round(_a * pow(10, _precision)) / pow(10, _precision);
     }
 
     /// \brief is this a power of 2?

gazebo/math/Pose.hh

       /// \return The resulting pose
       public: const Pose &operator+=(const Pose &_pose);
 
+      /// \brief Negation operator
+      /// \return The resulting pose
+      public: inline Pose operator-() const
+              {
+                return Pose() - *this;
+              }
+
       /// \brief Subtraction operator
       /// \param[in] _pose Pose to subtract from this one
       /// \return The resulting pose

gazebo/math/Quaternion.hh

 #include <iostream>
 #include <cmath>
 
-#include "math/Helpers.hh"
-#include "math/Angle.hh"
-#include "math/Vector3.hh"
-#include "math/Matrix3.hh"
-#include "math/Matrix4.hh"
+#include "gazebo/math/Helpers.hh"
+#include "gazebo/math/Angle.hh"
+#include "gazebo/math/Vector3.hh"
+#include "gazebo/math/Matrix3.hh"
+#include "gazebo/math/Matrix4.hh"
 
 namespace gazebo
 {
                 const gazebo::math::Quaternion &_q)
     {
       Vector3 v(_q.GetAsEuler());
-      _out << v.x << " " << v.y << " " << v.z;
+      _out << precision(v.x, 6) << " " << precision(v.y, 6) << " "
+           << precision(v.z, 6);
       return _out;
     }
 

gazebo/math/Vector3.hh

 #include <iostream>
 #include <fstream>
 
+#include "gazebo/math/Helpers.hh"
 #include "gazebo/common/CommonTypes.hh"
 
 namespace gazebo
       /// \param[in] _v vector to add
       public: const Vector3 &operator+=(const Vector3 &_v);
 
+      /// \brief Negation operator
+      /// \return negative of this vector
+      public: inline Vector3 operator-() const
+              {
+                return Vector3(-this->x, -this->y, -this->z);
+              }
+
       /// \brief Subtraction operators
       /// \param[in] _pt a vector to substract
       /// \return a vector
       public: friend std::ostream &operator<<(std::ostream &_out,
                                               const gazebo::math::Vector3 &_pt)
       {
-        _out << _pt.x << " " << _pt.y << " " << _pt.z;
+        _out << precision(_pt.x, 6) << " " << precision(_pt.y, 6) << " "
+             << precision(_pt.z, 6);
         return _out;
       }
 

gazebo/msgs/CMakeLists.txt

   joint_animation.proto
   joint_cmd.proto
   joint_wrench.proto
+  joint_wrench_stamped.proto
   laserscan.proto
   light.proto
   link.proto
   pose_animation.proto
   pose_stamped.proto
   pose_trajectory.proto
+  pose_v.proto
   projector.proto
   publishers.proto
   publish.proto

gazebo/msgs/joint_wrench_stamped.proto

+package gazebo.msgs;
+
+/// \ingroup gazebo_msgs
+/// \interface ForceTorque
+/// \brief ForceTorque from constraint solving
+
+
+import "joint_wrench.proto";
+import "time.proto";
+
+message ForceTorque
+{
+  repeated JointWrench wrench = 1;
+  required Time time          = 2;
+}
+

gazebo/msgs/pose.proto

   required Vector3d position      = 2;
   required Quaternion orientation = 3;
 }
-
-

gazebo/msgs/pose_v.proto

+package gazebo.msgs;
+
+/// \ingroup gazebo_msgs
+/// \interface Pose Vector
+/// \brief Message for a vector of poses
+
+import "pose.proto";
+
+message Pose_V
+{
+  repeated Pose pose = 1;
+}

gazebo/msgs/publish.proto

   required string host     = 3;
   required uint32 port     = 4;
 }
-
-

gazebo/physics/BoxShape.cc

 }
 
 //////////////////////////////////////////////////
-void BoxShape::FillShapeMsg(msgs::Geometry &_msg)
-{
-  this->FillMsg(_msg);
-}
-
-//////////////////////////////////////////////////
 void BoxShape::FillMsg(msgs::Geometry &_msg)
 {
   _msg.set_type(msgs::Geometry::BOX);
 {
   this->SetSize(msgs::Convert(_msg.box().size()));
 }
-
-//////////////////////////////////////////////////
-double BoxShape::GetMass(double _density) const
-{
-  math::Vector3 size = this->GetSize();
-  return size.x * size.y * size.z * _density;
-}
-
-//////////////////////////////////////////////////
-void BoxShape::GetInertial(double _mass, InertialPtr _inertial) const
-{
-  math::Vector3 size = this->GetSize();
-
-  _inertial->SetMass(_mass);
-  _inertial->SetIXX(_mass / 12.0 * (size.y * size.y + size.z * size.z));
-  _inertial->SetIYY(_mass / 12.0 * (size.x * size.x + size.z * size.z));
-  _inertial->SetIZZ(_mass / 12.0 * (size.x * size.x + size.y * size.y));
-}

gazebo/physics/BoxShape.hh

       /// \return The size of each side of the box.
       public: math::Vector3 GetSize() const;
 
-      /// \brief Deprecated.
-      public: void FillShapeMsg(msgs::Geometry &_msg) GAZEBO_DEPRECATED;
-
       /// \brief Fill in the values for a geomertry message.
       /// \param[out] _msg The geometry message to fill.
       public: void FillMsg(msgs::Geometry &_msg);
       /// \brief Process a geometry message.
       /// \param[in] _msg The message to set values from.
       public: virtual void ProcessMsg(const msgs::Geometry &_msg);
-
-      /// \brief Deprecated
-      public: virtual double GetMass(double _density) const GAZEBO_DEPRECATED;
-
-      /// \brief Deprecated
-      public: virtual void GetInertial(double _mass, InertialPtr _inertial)
-              const GAZEBO_DEPRECATED;
     };
     /// \}
   }

gazebo/physics/Collision.cc

 }
 
 //////////////////////////////////////////////////
-void Collision::FillCollisionMsg(msgs::Collision &_msg)
-{
-  this->FillMsg(_msg);
-}
-
-//////////////////////////////////////////////////
 void Collision::FillMsg(msgs::Collision &_msg)
 {
   msgs::Set(_msg.mutable_pose(), this->GetRelativePose());

gazebo/physics/Collision.hh

       public: void DisconnectContact(event::ConnectionPtr &_conn)
               {contact.Disconnect(_conn);}
 
-      /// \brief DEPRECATED.
-      public: void FillCollisionMsg(msgs::Collision &_msg) GAZEBO_DEPRECATED;
-
       /// \brief Fill a collision message.
       /// \param[out] _msg The message to fill with this collision's data.
       public: void FillMsg(msgs::Collision &_msg);

gazebo/physics/CollisionState.cc

 
   return result;
 }
+
+/////////////////////////////////////////////////
+void CollisionState::FillSDF(sdf::ElementPtr _sdf)
+{
+  _sdf->ClearElements();
+
+  _sdf->GetAttribute("name")->Set(this->name);
+  _sdf->GetElement("pose")->Set(this->pose);
+}
+

gazebo/physics/CollisionState.hh

       /// \return True if the values in the state are zero.
       public: bool IsZero() const;
 
+      /// \brief Populate a state SDF element with data from the object.
+      /// \param[out] _sdf SDF element to populate.
+      public: void FillSDF(sdf::ElementPtr _sdf);
+
       /// \brief Assignment operator
       /// \param[in] _state State value
       /// \return Reference to this

gazebo/physics/Contact.cc

 }
 
 //////////////////////////////////////////////////
-Contact Contact::Clone() const
-{
-  return Contact(*this);
-}
-
-//////////////////////////////////////////////////
 Contact &Contact::operator =(const Contact &_contact)
 {
   this->collision1 = _contact.collision1;

gazebo/physics/Contact.hh

       /// \brief Destructor.
       public: virtual ~Contact();
 
-      /// \brief Deprecated
-      public: Contact Clone() const GAZEBO_DEPRECATED;
-
       /// \brief Operator =.
       /// \param[in] _contact Contact to copy.
       /// \return Reference to this contact

gazebo/physics/CylinderShape.cc

   _msg.mutable_cylinder()->set_length(this->GetLength());
 }
 
+/////////////////////////////////////////////////
 void CylinderShape::ProcessMsg(const msgs::Geometry &_msg)
 {
   this->SetSize(_msg.cylinder().radius(), _msg.cylinder().length());
 }
-
-//////////////////////////////////////////////////
-double CylinderShape::GetMass(double _density) const
-{
-  double r = this->GetRadius();
-  double l = this->GetLength();
-  return M_PI * r * r * l * _density;
-}
-
-//////////////////////////////////////////////////
-void CylinderShape::GetInertial(double _mass, InertialPtr _inertial) const
-{
-  double r = this->GetRadius();
-  double l = this->GetLength();
-
-  double r2 = r * r;
-  double i = _mass * (0.25 * r2 + (1.0/12.0) * l * l);
-
-  _inertial->SetMass(_mass);
-  _inertial->SetIXX(i);
-  _inertial->SetIYY(i);
-  // cylinders are oriented along the z axis
-  _inertial->SetIZZ(_mass * 0.5 * r2);
-}

gazebo/physics/CylinderShape.hh

       /// \brief Update values based on a message.
       /// \param[in] _msg Message to update from.
       public: virtual void ProcessMsg(const msgs::Geometry &_msg);
-
-      /// \brief Deprecated.
-      public: virtual double GetMass(double _density) const GAZEBO_DEPRECATED;
-
-      /// \brief Deprecated.
-      public: virtual void GetInertial(double _mass, InertialPtr _inertial)
-              const GAZEBO_DEPRECATED;
     };
     /// \}
   }

gazebo/physics/Entity.cc

   delete this->poseMsg;
   this->poseMsg = NULL;
 
-  this->posePub.reset();
   this->visPub.reset();
   this->requestPub.reset();
   this->poseSub.reset();
 {
   Base::Load(_sdf);
   this->node->Init(this->GetWorld()->GetName());
-  this->posePub = this->node->Advertise<msgs::Pose>("~/pose/info", 10);
 
   this->poseSub = this->node->Subscribe("~/pose/modify",
       &Entity::OnPoseMsg, this);
 //////////////////////////////////////////////////
 void Entity::PublishPose()
 {
-  if (this->posePub && this->posePub->HasConnections())
+  math::Pose relativePose = this->GetRelativePose();
+  if (relativePose != msgs::Convert(*this->poseMsg))
   {
-    math::Pose relativePose = this->GetRelativePose();
-    if (relativePose != msgs::Convert(*this->poseMsg))
-    {
-      msgs::Set(this->poseMsg, relativePose);
-      this->posePub->Publish(*this->poseMsg);
-    }
+    msgs::Set(this->poseMsg, relativePose);
+    this->world->EnqueueMsg(this->poseMsg);
   }
 }
 

gazebo/physics/Entity.hh

       /// \brief Visual message container.
       protected: msgs::Visual *visualMsg;
 
-      /// \brief Pose message containr.
+      /// \brief Pose message container.
       protected: msgs::Pose *poseMsg;
 
       /// \brief Current pose animation
       /// \brief The function used to to set the world pose.
       private: void (Entity::*setWorldPoseFunc)(const math::Pose &, bool, bool);
     };
-
     /// \}
   }
 }

gazebo/physics/Gripper.cc

       if (collIter != this->collisions.end())
         continue;
       collision->SetContactsEnabled(true);
-      this->connections.push_back(collision->ConnectContact(
-            boost::bind(&Gripper::OnContact, this, _1, _2)));
+
       this->collisions[collision->GetScopedName()] = collision;
     }
     gripperLinkElem = gripperLinkElem->GetNextElement("gripper_link");
           this->attached = true;
 
           this->fixedJoint->Load(this->palmLink,
-              cc[iter->first]->GetLink(), math::Pose(0, 0, 0, 0, 0, 0));
+              cc[iter->first]->GetLink(), math::Vector3(0, 0, 0));
           this->fixedJoint->Init();
           this->fixedJoint->SetHighStop(0, 0);
           this->fixedJoint->SetLowStop(0, 0);

gazebo/physics/Joint.cc

   : Base(_parent)
 {
   this->AddType(Base::JOINT);
+  this->forceApplied[0] = 0;
+  this->forceApplied[1] = 0;
 }
 
 //////////////////////////////////////////////////
 }
 
 //////////////////////////////////////////////////
-void Joint::Load(LinkPtr _parent, LinkPtr _child, const math::Pose &_pose)
+void Joint::Load(LinkPtr _parent, LinkPtr _child, const math::Vector3 &_pos)
 {
   if (_parent)
   {
 
   this->parentLink = _parent;
   this->childLink = _child;
-  this->LoadImpl(_pose);
+  this->anchorPos = _pos;
+}
+
+//////////////////////////////////////////////////
+void Joint::Load(LinkPtr _parent, LinkPtr _child, const math::Pose &_pose)
+{
+  this->Load(_parent, _child, _pose.pos);
 }
 
 //////////////////////////////////////////////////
   std::string parentName = _sdf->GetValueString("parent");
   std::string childName = _sdf->GetValueString("child");
 
-  math::Pose pose;
-  if (_sdf->HasElement("pose"))
-    pose = _sdf->GetValuePose("pose");
-
   if (this->model)
   {
     this->childLink = this->model->GetLink(childName);
   if (!this->childLink && childName != std::string("world"))
     gzthrow("Couldn't Find Child Link[" + childName  + "]");
 
-  this->LoadImpl(pose);
+  /// \todo: FIXME either element pose need to be converted to xyz because
+  /// that's all we use here, or we need to use the rotational part
+  /// of pose.  But rotational part of the pose is redundanct with
+  /// specification of <axis> for joints, it just adds complexity,
+  /// so we should make <pose> a <position>.
+  if (_sdf->HasElement("pose"))
+    this->anchorPos = _sdf->GetValuePose("pose").pos;
+
+  // this->LoadImpl( _sdf->GetValuePose("pose"));
 }
 
 /////////////////////////////////////////////////
     this->parentLink->AddChildJoint(boost::shared_static_cast<Joint>(myBase));
   this->childLink->AddParentJoint(boost::shared_static_cast<Joint>(myBase));
 
+  /// \todo: FIXME either convert _pose to Vector3 because
+  /// that's all we use here, or we need to use the rotational part
+  /// of _pose.
   // setting anchor relative to gazebo link frame pose
   if (this->childLink)
     this->anchorPos = (_pose + this->childLink->GetWorldPose()).pos;
   this->Attach(this->parentLink, this->childLink);
 
   // Set the anchor vector
-  this->SetAnchor(0, this->anchorPos);
+  this->SetAnchor(0, this->anchorPos + this->childLink->GetWorldPose().pos);
 
   if (this->sdf->HasElement("axis"))
   {
 }
 
 //////////////////////////////////////////////////
-void Joint::FillJointMsg(msgs::Joint &_msg)
-{
-  this->FillMsg(_msg);
-}
-
-//////////////////////////////////////////////////
 void Joint::FillMsg(msgs::Joint &_msg)
 {
   _msg.set_name(this->GetScopedName());
 }
 
 //////////////////////////////////////////////////
-void Joint::SetForce(int /*_index*/, double /*_force*/)
+void Joint::SetForce(int _index, double _force)
 {
+  /// \todo: should check to see if this type of joint has _index
+  if (_index < 2)
+    this->forceApplied[_index] = _force;
+  else
+    gzerr << "Invalid joint index [" << _index
+          << "] when trying to apply force\n";
 }
 
 //////////////////////////////////////////////////
-double Joint::GetForce(int /*_index*/)
+double Joint::GetForce(int _index)
 {
-  return 0;
+  /// \todo: should check to see if this type of joint has _index
+  if (_index < 2)
+    return this->forceApplied[_index];
+  else
+  {
+    gzerr << "Invalid joint index [" << _index
+          << "] when trying to apply force\n";
+    return 0;
+  }
 }
 
 //////////////////////////////////////////////////

gazebo/physics/Joint.hh

 
 #include "gazebo/physics/JointState.hh"
 #include "gazebo/physics/Base.hh"
+#include "gazebo/physics/JointWrench.hh"
 
 namespace gazebo
 {
       /// \brief Set pose, parent and child links of a physics::Joint
       /// \param[in] _parent Parent link.
       /// \param[in] _child Child link.
-      /// \param[in] _pose Pose of the link.
+      /// \param[in] _pose Pose containing Joint Anchor offset from child link.
       public: void Load(LinkPtr _parent, LinkPtr _child,
-                        const math::Pose &_pose);
+                        const math::Pose &_pose) GAZEBO_DEPRECATED;
+
+      /// \brief Set parent and child links of a physics::Joint and its
+      /// anchor offset position.
+      /// \param[in] _parent Parent link.
+      /// \param[in] _child Child link.
+      /// \param[in] _pos Joint Anchor offset from child link.
+      public: void Load(LinkPtr _parent, LinkPtr _child,
+                        const math::Vector3 &_pos);
 
       /// \brief Load physics::Joint from a SDF sdf::Element.
       /// \param[in] _sdf SDF values to load from.
       /// \return The force applied to an axis.
       public: virtual double GetForce(int _index);
 
+      /// \brief get force torque values at a joint
+      /// \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
+      public: virtual JointWrench GetForceTorque(int _index) = 0;
+
       /// \brief Set the max allowed force of an axis(index).
       /// Note that the unit of force should be consistent with the rest
       /// of the simulation scales.  E.g.  if you are using
       public: virtual math::Vector3 GetLinkTorque(
                   unsigned int _index) const = 0;
 
-      /// \brief Set a parameter for the joint.
-      /// \param[in] _attr Attribute to set.
-      /// \param[in] _index Index of the axis.
-      /// \param[in] _value Value of the attribute.
-      public: virtual void SetAttribute(Attribute _attr, int _index,
-                                        double _value) GAZEBO_DEPRECATED = 0;
-
       /// \brief Set a non-generic parameter for the joint.
       /// replaces SetAttribute(Attribute, int, double)
       /// \param[in] _key String key.
       /// \return Pointer to the parent link.
       public: LinkPtr GetParent() const;
 
-      /// \brief DEPRECATED
-      /// \param[out] _msg Message to fill with joint's properties
-      /// \sa Joint::FillMsg
-      public: void FillJointMsg(msgs::Joint &_msg) GAZEBO_DEPRECATED;
-
       /// \brief Fill a joint message.
       /// \param[out] _msg Message to fill with this joint's properties.
       public: void FillMsg(msgs::Joint &_msg);
 
       /// \brief Helper function to load a joint.
       /// \param[in] _pose Pose of the anchor.
-      private: void LoadImpl(const math::Pose &_pose);
+      private: void LoadImpl(const math::Pose &_pose) GAZEBO_DEPRECATED;
 
       /// \brief The first link this joint connects to
       protected: LinkPtr childLink;
 
       /// \brief apply damping for adding viscous damping forces on updates
       protected: gazebo::event::ConnectionPtr applyDamping;
+
+      /// \brief Save force applied by user
+      /// This plus the joint feedback (joint contstraint forces) is the
+      /// equivalent of simulated force torque sensor reading
+      /// Allocate a 2 vector in case hinge2 joint is used.
+      protected: double forceApplied[2];
     };
     /// \}
   }

gazebo/physics/JointState.cc

 
   return result;
 }
+
+/////////////////////////////////////////////////
+void JointState::FillSDF(sdf::ElementPtr _sdf)
+{
+  _sdf->ClearElements();
+
+  _sdf->GetAttribute("name")->Set(this->name);
+
+  int i = 0;
+  for (std::vector<math::Angle>::const_iterator iter = this->angles.begin();
+       iter != this->angles.end(); ++iter, ++i)
+  {
+    sdf::ElementPtr elem = _sdf->AddElement("angle");
+    elem->GetAttribute("axis")->Set(i);
+    elem->Set((*iter).Radian());
+  }
+}

gazebo/physics/JointState.hh

       /// \return True if the values in the state are zero.
       public: bool IsZero() const;
 
+      /// \brief Populate a state SDF element with data from the object.
+      /// \param[out] _sdf SDF element to populate.
+      public: void FillSDF(sdf::ElementPtr _sdf);
+
       /// \brief Assignment operator
       /// \param[in] _state State value
       /// \return this

gazebo/physics/JointWrench.hh

  * Date: 10 Nov 2009
  */
 
-#ifndef _JOINTFEEDBACK_HH_
-#define _JOINTFEEDBACK_HH_
+#ifndef _JOINT_WRENCH_HH_
+#define _JOINT_WRENCH_HH_
 
 #include "math/Vector3.hh"
 

gazebo/physics/Link.cc

 }
 
 //////////////////////////////////////////////////
-void Link::FillLinkMsg(msgs::Link &_msg)
-{
-  this->FillMsg(_msg);
-}
-
-//////////////////////////////////////////////////
 void Link::FillMsg(msgs::Link &_msg)
 {
   _msg.set_id(this->GetId());

gazebo/physics/Link.hh

       public: void DisconnectEnabled(event::ConnectionPtr &_conn)
               {enabledSignal.Disconnect(_conn);}
 
-      /// \brief DEPRECATED
-      public: void FillLinkMsg(msgs::Link &_msg) GAZEBO_DEPRECATED;
-
       /// \brief Fill a link message
       /// \param[out] _msg Message to fill
       public: void FillMsg(msgs::Link &_msg);

gazebo/physics/LinkState.cc

 
   return result;
 }
+
+/////////////////////////////////////////////////
+void LinkState::FillSDF(sdf::ElementPtr _sdf)
+{
+  _sdf->ClearElements();
+
+  _sdf->GetAttribute("name")->Set(this->name);
+  _sdf->GetElement("pose")->Set(this->pose);
+  _sdf->GetElement("velocity")->Set(this->velocity);
+  _sdf->GetElement("acceleration")->Set(this->acceleration);
+  _sdf->GetElement("wrench")->Set(this->wrench);
+
+  for (std::vector<CollisionState>::iterator iter =
+       this->collisionStates.begin();
+       iter != this->collisionStates.end(); ++iter)
+  {
+    sdf::ElementPtr elem = _sdf->AddElement("collision");
+    (*iter).FillSDF(elem);
+  }
+}

gazebo/physics/LinkState.hh

       /// \return True if the values in the state are zero.
       public: bool IsZero() const;
 
+      /// \brief Populate a state SDF element with data from the object.
+      /// \param[out] _sdf SDF element to populate.
+      public: void FillSDF(sdf::ElementPtr _sdf);
+
       /// \brief Assignment operator
       /// \param[in] _state State value
       /// \return this

gazebo/physics/Model.cc

       boost::bind(&Entity::IsStatic, this));
 
   this->SetAutoDisable(this->sdf->GetValueBool("allow_auto_disable"));
+  this->LoadLinks();
 
+  // Load the joints if the world is already loaded. Otherwise, the World
+  // has some special logic to load models that takes into account state
+  // information.
+  if (this->world->IsLoaded())
+    this->LoadJoints();
+}
+
+//////////////////////////////////////////////////
+void Model::LoadLinks()
+{
   /// \TODO: check for duplicate model, and raise an error
   /// BasePtr dup = Base::GetByName(this->GetScopedName());
 
   // Load the bodies
-  if (_sdf->HasElement("link"))
+  if (this->sdf->HasElement("link"))
   {
-    sdf::ElementPtr linkElem = _sdf->GetElement("link");
+    sdf::ElementPtr linkElem = this->sdf->GetElement("link");
     bool canonicalLinkInitialized = false;
     while (linkElem)
     {
       linkElem = linkElem->GetNextElement("link");
     }
   }
+}
 
+//////////////////////////////////////////////////
+void Model::LoadJoints()
+{
   // Load the joints
-  if (_sdf->HasElement("joint"))
+  if (this->sdf->HasElement("joint"))
   {
-    sdf::ElementPtr jointElem = _sdf->GetElement("joint");
+    sdf::ElementPtr jointElem = this->sdf->GetElement("joint");
     while (jointElem)
     {
       try
     }
   }
 
-  if (_sdf->HasElement("gripper"))
+  if (this->sdf->HasElement("gripper"))
   {
-    sdf::ElementPtr gripperElem = _sdf->GetElement("gripper");
+    sdf::ElementPtr gripperElem = this->sdf->GetElement("gripper");
     while (gripperElem)
     {
       this->LoadGripper(gripperElem);
 }
 
 //////////////////////////////////////////////////
-JointPtr Model::GetJoint(unsigned int _index) const
-{
-  if (_index >= this->joints.size())
-    gzthrow("Invalid joint _index[" << _index << "]\n");
-
-  return this->joints[_index];
-}
-
-//////////////////////////////////////////////////
 JointPtr Model::GetJoint(const std::string &_name)
 {
   JointPtr result;
 }
 
 //////////////////////////////////////////////////
-Link_V Model::GetAllLinks() const
-{
-  return this->GetLinks();
-}
-
-//////////////////////////////////////////////////
 Link_V Model::GetLinks() const
 {
   Link_V links;
 }
 
 //////////////////////////////////////////////////
-LinkPtr Model::GetLink(unsigned int _index) const
-{
-  LinkPtr link;
-  if (_index <= this->GetChildCount())
-    link = boost::shared_static_cast<Link>(this->GetChild(_index));
-  else
-    gzerr << "Index is out of range\n";
-
-  return link;
-}
-
-//////////////////////////////////////////////////
 void Model::LoadJoint(sdf::ElementPtr _sdf)
 {
   JointPtr joint;
 }
 
 //////////////////////////////////////////////////
-void Model::FillModelMsg(msgs::Model &_msg)
-{
-  this->FillMsg(_msg);
-}
-
-//////////////////////////////////////////////////
 void Model::FillMsg(msgs::Model &_msg)
 {
   _msg.set_name(this->GetScopedName());
     JointState jointState = _state.GetJointState(i);
     this->SetJointPosition(this->GetName() + "::" + jointState.GetName(),
                            jointState.GetAngle(0).Radian());
-
-    /*JointPtr joint = this->GetJoint(jointState.GetName());
-    if (joint)
-      joint->SetState(jointState);
-    else
-      gzerr << "Unable to find joint[" << jointState.GetName() << "]\n";
-      */
   }
 }
 

gazebo/physics/Model.hh

       /// \param[in] _sdf SDF parameters to load from.
       public: void Load(sdf::ElementPtr _sdf);
 
+      /// \brief Load all the joints.
+      public: void LoadJoints();
+
       /// \brief Initialize the model.
       public: virtual void Init();
 
       /// \return Get the number of joints.
       public: unsigned int GetJointCount() const;
 
-      /// Deprecated
-      public: Link_V GetAllLinks() const GAZEBO_DEPRECATED;
-
       /// \brief Construct and return a vector of Link's in this model
       /// Note this constructs the vector of Link's on the fly, could be costly
       /// \return a vector of Link's in this model
       public: Link_V GetLinks() const;
 
-      /// \brief Get a joint by index
-      /// \param index Index of the joint
-      /// \return A pointer to the joint
-      public: JointPtr GetJoint(unsigned int index) const GAZEBO_DEPRECATED;
-
       /// \brief Get the joints.
       /// \return Vector of joints.
       public: const Joint_V &GetJoints() const;
       /// \return Pointer to the link, NULL if the name is invalid.
       public: LinkPtr GetLink(const std::string &_name ="canonical") const;
 
-      /// \brief This function is dangerous. Do not use.
-      public: LinkPtr GetLink(unsigned int _index) const GAZEBO_DEPRECATED;
-
       /// \brief Set the gravity mode of the model.
       /// \param[in] _value False to turn gravity on for the model.
       public: void SetGravityMode(const bool &_value);
       /// \param[in] _retro Retro reflectance value.
       public: void SetLaserRetro(const float _retro);
 
-      /// \brief DEPRECATED
-      public: void FillModelMsg(msgs::Model &_msg) GAZEBO_DEPRECATED;
-
       /// \brief Fill a model message.
       /// \param[in] _msg Message to fill using this model's data.
       public: void FillMsg(msgs::Model &_msg);
       /// \brief Callback when the pose of the model has been changed.
       protected: virtual void OnPoseChange();
 
+      /// \brief Load all the links.
+      private: void LoadLinks();
+
       /// \brief Load a joint helper function.
       /// \param[in] _sdf SDF parameter.
       private: void LoadJoint(sdf::ElementPtr _sdf);

gazebo/physics/ModelState.cc

 
   return result;
 }
+
+/////////////////////////////////////////////////
+void ModelState::FillSDF(sdf::ElementPtr _sdf)
+{
+  _sdf->ClearElements();
+
+  _sdf->GetAttribute("name")->Set(this->name);
+  _sdf->GetElement("pose")->Set(this->pose);
+
+  for (std::vector<LinkState>::iterator iter = this->linkStates.begin();
+       iter != this->linkStates.end(); ++iter)
+  {
+    sdf::ElementPtr elem = _sdf->AddElement("link");
+    (*iter).FillSDF(elem);
+  }
+
+  for (std::vector<JointState>::iterator iter = this->jointStates.begin();
+       iter != this->jointStates.end(); ++iter)
+  {
+    sdf::ElementPtr elem = _sdf->AddElement("joint");
+    (*iter).FillSDF(elem);
+  }
+}

gazebo/physics/ModelState.hh

       /// \return A vector of joint states.
       public: const std::vector<JointState> &GetJointStates() const;
 
+      /// \brief Populate a state SDF element with data from the object.
+      /// \param[out] _sdf SDF element to populate.
+      public: void FillSDF(sdf::ElementPtr _sdf);
+
       /// \brief Assignment operator
       /// \param[in] _state State value
       /// \return this

gazebo/physics/Road.cc

   this->node = transport::NodePtr(new transport::Node());
   this->node->Init("default");
 
-  this->roadPub = this->node->Advertise<msgs::Road>("~/roads", 10, true);
+  this->roadPub = this->node->Advertise<msgs::Road>("~/roads", 10);
 
   msgs::Road msg;
   Base::Load(_elem);

gazebo/physics/Shape.cc

   if (this->collisionParent)
     this->collisionParent->SetShape(ShapePtr());
 }
-
-//////////////////////////////////////////////////
-void Shape::GetInertial(double /*_mass*/, InertialPtr /*_inertial*/) const
-{
-  return;
-}
-
-//////////////////////////////////////////////////
-void Shape::FillShapeMsg(msgs::Geometry &_msg)
-{
-  this->FillMsg(_msg);
-}

gazebo/physics/Shape.hh

       /// \brief Initialize the shape.
       public: virtual void Init() = 0;
 
-      /// \brief Deprecated.
-      public: virtual double GetMass(double _density) const GAZEBO_DEPRECATED
-              {return _density;}
-
-      /// \brief Deprecated
-      public: virtual void GetInertial(double _mass, InertialPtr _inertial)
-              const GAZEBO_DEPRECATED;
-
-      /// \brief Deprecated
-      public: virtual void FillShapeMsg(msgs::Geometry &_msg)
-              GAZEBO_DEPRECATED;
-
       /// \brief Fill in the values for a geometry message.
       /// \param[out] _msg The geometry message to fill.
       public: virtual void FillMsg(msgs::Geometry &_msg) = 0;

gazebo/physics/SphereShape.cc

 {
   this->SetRadius(_msg.sphere().radius());
 }
-
-//////////////////////////////////////////////////
-double SphereShape::GetMass(double _density) const
-{
-  double r = this->GetRadius();
-  return (4.0/3.0) * M_PI * r * r * r * _density;
-}
-
-//////////////////////////////////////////////////
-void SphereShape::GetInertial(double _mass, InertialPtr _inertial) const
-{
-  double r = this->GetRadius();
-  double ii = 0.4 * _mass * r * r;
-
-  _inertial->SetMass(_mass);
-  _inertial->SetIXX(ii);
-  _inertial->SetIYY(ii);
-  _inertial->SetIZZ(ii);
-}

gazebo/physics/SphereShape.hh

       /// \brief Process a geometry message.
       /// \param[in] _msg The message to set values from.
       public: virtual void ProcessMsg(const msgs::Geometry &_msg);
-
-      /// \brief Deprecated.
-      public: virtual double GetMass(double _density) const GAZEBO_DEPRECATED;
-
-      /// \brief Deprecated.
-      public: virtual void GetInertial(double _mass, InertialPtr _inertial)
-              const GAZEBO_DEPRECATED;
     };
     /// \}
   }

gazebo/physics/SurfaceParams.cc

 }
 
 /////////////////////////////////////////////////
-void SurfaceParams::FillSurfaceMsg(msgs::Surface &_msg)
-{
-  this->FillMsg(_msg);
-}
-
-/////////////////////////////////////////////////
 void SurfaceParams::FillMsg(msgs::Surface &_msg)
 {
   _msg.mutable_friction()->set_mu(this->mu1);

gazebo/physics/SurfaceParams.hh

       /// \param[in] _sdf SDF values to load from.
       public: virtual void Load(sdf::ElementPtr _sdf);
 
-      /// \brief Deprecated.
-      public: void FillSurfaceMsg(msgs::Surface &_msg) GAZEBO_DEPRECATED;
-
       /// \brief Fill in a surface message.
       /// \param[in] _msg Message to fill with this object's values.
       public: void FillMsg(msgs::Surface &_msg);

gazebo/physics/TrimeshShape.cc

 
   this->mesh = NULL;
   common::MeshManager *meshManager = common::MeshManager::Instance();
-  if (this->sdf->GetValueString("filename") != "__default__")
+
+  filename = common::find_file(this->sdf->GetValueString("uri"));
+
+  if (filename == "__default__" || filename.empty())
   {
-    filename = common::find_file(this->sdf->GetValueString("filename"));
-
-    gzerr << "<mesh><filename>" << filename << "</filename></mesh>"
-          << " is deprecated.\n";
-    gzerr << "Use <mesh><uri>file://" << filename << "</uri></mesh>\n";
-    this->sdf->GetElement("uri")->Set(std::string("file://") + filename);
-  }
-  else
-  {
-    filename = common::find_file(this->sdf->GetValueString("uri"));
-    if (filename == "__default__" || filename.empty())
-    {
-      gzerr << "No mesh specified\n";
-      return;
-    }
+    gzerr << "No mesh specified\n";
+    return;
   }
 
   if ((this->mesh = meshManager->Load(filename)) == NULL)
   this->SetScale(msgs::Convert(_msg.mesh().scale()));
   this->SetFilename(_msg.mesh().filename());
 }
-
-//////////////////////////////////////////////////
-double TrimeshShape::GetMass(double _density) const
-{
-  gzerr << "Not implemented\n";
-  return _density;
-}

gazebo/physics/TrimeshShape.hh

       /// \param[in] _msg Message that contains triangle mesh info.
       public: virtual void ProcessMsg(const msgs::Geometry &_msg);
 
-      /// \brief Deprecated
-      public: virtual double GetMass(double _density) const GAZEBO_DEPRECATED;
-
       /// \brief Pointer to the mesh data.
       protected: const common::Mesh *mesh;
     };

gazebo/physics/World.cc

   this->logPlayStateSDF.reset(new sdf::Element);
   sdf::initFile("state.sdf", this->logPlayStateSDF);
 
-  this->receiveMutex = new boost::mutex();
+  this->receiveMutex = new boost::recursive_mutex();
   this->loadModelMutex = new boost::mutex();
 
   this->initialized = false;
+  this->loaded = false;
   this->stepInc = 0;
   this->pause = false;
   this->thread = NULL;
 //////////////////////////////////////////////////
 void World::Load(sdf::ElementPtr _sdf)
 {
+  this->loaded = false;
   this->sdf = _sdf;
 
   if (this->sdf->GetValueString("name").empty())
   this->node = transport::NodePtr(new transport::Node());
   this->node->Init(this->GetName());
 
-  this->guiPub = this->node->Advertise<msgs::GUI>("~/gui", 1, true);
+  this->posePub = this->node->Advertise<msgs::Pose_V>("~/pose/info", 10);
+
+  this->guiPub = this->node->Advertise<msgs::GUI>("~/gui");
   if (this->sdf->HasElement("gui"))
     this->guiPub->Publish(msgs::GUIFromSDF(this->sdf->GetElement("gui")));
 
   this->rootElement->SetName(this->GetName());
   this->rootElement->SetWorld(shared_from_this());
 
-  if (this->sdf->HasElement("state"))
+  // A special order is necessary when loading a world that contains state
+  // information. The joints must be created last, otherwise they get
+  // initialized improperly.
   {
-    sdf::ElementPtr childElem = this->sdf->GetElement("state");
+    // Create all the entities
+    this->LoadEntities(this->sdf, this->rootElement);
 
-    while (childElem)
+    // Set the state of the entities
+    if (this->sdf->HasElement("state"))
     {
-      WorldState myState;
-      myState.Load(childElem);
-      this->sdf->InsertElement(childElem);
-      // this->SetState(myState);
+      sdf::ElementPtr childElem = this->sdf->GetElement("state");
 
-      childElem = childElem->GetNextElement("state");
+      while (childElem)
+      {
+        WorldState myState;
+        myState.Load(childElem);
+        this->SetState(myState);
 
-      // TODO: We currently load just the first state data. Need to
-      // implement a better mechanism for handling multiple states
-      break;
+        childElem = childElem->GetNextElement("state");
+
+        // TODO: We currently load just the first state data. Need to
+        // implement a better mechanism for handling multiple states
+        break;
+      }
     }
+
+    for (unsigned int i = 0; i < this->GetModelCount(); ++i)
+      this->GetModel(i)->LoadJoints();
   }
 
-  // Create all the entities
-  this->LoadEntities(this->sdf, this->rootElement);
-
   // TODO: Performance test to see if TBB model updating is necessary
   // Choose threaded or unthreaded model updating depending on the number of
   // models in the scene
   // this->modelUpdateFunc = &World::ModelUpdateTBB;
 
   event::Events::worldCreated(this->GetName());
+
+  this->loaded = true;
 }
 
-
 //////////////////////////////////////////////////
 void World::Save(const std::string &_filename)
 {
   // exponentially avg out
   this->sleepOffset = (actualSleep - sleepTime) * 0.01 +
                       this->sleepOffset * 0.99;
+
   // throttling update rate, with sleepOffset as tolerance
   // the tolerance is needed as the sleep time is not exact
   if (common::Time::GetWallTime() - this->prevStepWallTime + this->sleepOffset
          >= common::Time(this->physicsEngine->GetUpdatePeriod()))
   {
-    this->worldUpdateMutex->lock();
+    boost::recursive_mutex::scoped_lock lock(*this->worldUpdateMutex);
 
     this->prevStepWallTime = common::Time::GetWallTime();
 
     }
     else
       this->pauseTime += this->physicsEngine->GetStepTime();
-
-    this->worldUpdateMutex->unlock();
   }
 
   this->ProcessMessages();
     this->SetPaused(true);
   }
 
-  this->worldUpdateMutex->lock();
-  this->stepInc = _steps;
-  this->worldUpdateMutex->unlock();
+  {
+    boost::recursive_mutex::scoped_lock lock(*this->worldUpdateMutex);
+    this->stepInc = _steps;
+  }
 
   // block on completion
   bool wait = true;
   while (wait)
   {
     common::Time::MSleep(1);
-    this->worldUpdateMutex->lock();
+    boost::recursive_mutex::scoped_lock lock(*this->worldUpdateMutex);
     if (this->stepInc == 0 || this->stop)
       wait = false;
-    this->worldUpdateMutex->unlock();
   }
 }
 
 //////////////////////////////////////////////////
 unsigned int World::GetModelCount() const
 {
-  return this->rootElement->GetChildCount();
+  // Not all children of the root element are models. We have to iterate
+  // through the children and count only the models.
+  unsigned int result = 0;
+  for (unsigned int i = 0; i < this->rootElement->GetChildCount(); i++)
+    result += this->rootElement->GetChild(i)->HasType(Base::MODEL) ? 1 : 0;
+  return result;
 }
 
 //////////////////////////////////////////////////
 {
   ModelPtr model;
 
-  if (_index < this->rootElement->GetChildCount() &&
-      this->rootElement->GetChild(_index)->HasType(Base::MODEL))
+  if (_index < this->rootElement->GetChildCount())
   {
-    model =
-      boost::shared_static_cast<Model>(this->rootElement->GetChild(_index));
+    // Not all children of the root element are models. We have to iterate
+    // through the children and count only the models.
+    for (unsigned int i = 0, count = 0;
+         i < this->rootElement->GetChildCount(); i++)
+    {
+      if (this->rootElement->GetChild(i)->HasType(Base::MODEL))
+      {
+        if (count == _index)
+        {
+          model = boost::shared_static_cast<Model>(
+              this->rootElement->GetChild(i));
+          break;
+        }
+        count++;
+      }
+    }
+
+    if (!model)
+      gzerr << "Unable to get model with index[" << _index << "]\n";
   }
   else
   {
-    gzerr << "Invalid model index\n";
+    gzerr << "Given model index[" << _index << "] is out of range[0.."
+          << this->rootElement->GetChildCount() << "]\n";
   }
 
   return model;
 {
   bool currently_paused = this->IsPaused();
   this->SetPaused(true);
-  this->worldUpdateMutex->lock();
 
-  this->ResetTime();
-  this->ResetEntities(Base::BASE);
-  for (std::vector<WorldPluginPtr>::iterator iter = this->plugins.begin();
-       iter != this->plugins.end(); ++iter)
-    (*iter)->Reset();
-  this->physicsEngine->Reset();
+  {
+    boost::recursive_mutex::scoped_lock(*this->worldUpdateMutex);
 
-  this->worldUpdateMutex->unlock();
+    this->ResetTime();
+    this->ResetEntities(Base::BASE);
+    for (std::vector<WorldPluginPtr>::iterator iter = this->plugins.begin();
+        iter != this->plugins.end(); ++iter)
+      (*iter)->Reset();
+    this->physicsEngine->Reset();
+  }
+
   this->SetPaused(currently_paused);
 }
 
   if (this->pause == _p)
     return;
 
-  this->worldUpdateMutex->lock();
-  this->pause = _p;
-  this->worldUpdateMutex->unlock();
+  {
+    boost::recursive_mutex::scoped_lock(*this->worldUpdateMutex);
+    this->pause = _p;
+  }
 
   if (_p)
     this->pauseStartTime = common::Time::GetWallTime();
 //////////////////////////////////////////////////
 void World::OnFactoryMsg(ConstFactoryPtr &_msg)
 {
-  boost::mutex::scoped_lock lock(*this->receiveMutex);
+  boost::recursive_mutex::scoped_lock lock(*this->receiveMutex);
   this->factoryMsgs.push_back(*_msg);
 }
 
 //////////////////////////////////////////////////
 void World::OnRequest(ConstRequestPtr &_msg)
 {
-  boost::mutex::scoped_lock lock(*this->receiveMutex);
+  boost::recursive_mutex::scoped_lock lock(*this->receiveMutex);
   this->requestMsgs.push_back(*_msg);
 }
 
 //////////////////////////////////////////////////
 void World::JointLog(ConstJointPtr &_msg)
 {
-  boost::mutex::scoped_lock lock(*this->receiveMutex);
+  boost::recursive_mutex::scoped_lock lock(*this->receiveMutex);
   int i = 0;
   for (; i < this->sceneMsg.joint_size(); i++)
   {
 //////////////////////////////////////////////////
 void World::OnModelMsg(ConstModelPtr &_msg)
 {
-  boost::mutex::scoped_lock lock(*this->receiveMutex);
+  boost::recursive_mutex::scoped_lock lock(*this->receiveMutex);
   this->modelMsgs.push_back(*_msg);
 }
 
 //////////////////////////////////////////////////
 void World::ProcessEntityMsgs()
 {
-  boost::mutex::scoped_lock lock(*this->receiveMutex);
-
   std::list<std::string>::iterator iter;
   for (iter = this->deleteEntity.begin();
        iter != this->deleteEntity.end(); ++iter)
 //////////////////////////////////////////////////
 void World::ProcessRequestMsgs()
 {
-  boost::mutex::scoped_lock lock(*this->receiveMutex);
   msgs::Response response;
 
   std::list<msgs::Request>::iterator iter;
     {
       msgs::GzString msg;
       this->UpdateStateSDF();
-      std::string data;
-      data = "<?xml version='1.0'?>\n";
-      data += "<sdf version='" +
-        boost::lexical_cast<std::string>(SDF_VERSION) + "'>\n";
-      data += this->sdf->ToString("");
-      data += "</sdf>\n";
-      msg.set_data(data);
+      std::ostringstream stream;
+      stream << "<?xml version='1.0'?>\n"
+             << "<sdf version='" << SDF_VERSION << "'>\n"
+             << this->sdf->ToString("")
+             << "</sdf>";
+
+      msg.set_data(stream.str());
 
       std::string *serializedData = response.mutable_serialized_data();
       msg.SerializeToString(serializedData);
 void World::ProcessModelMsgs()
 {
   std::list<msgs::Model>::iterator iter;
-  boost::mutex::scoped_lock lock(*this->receiveMutex);
   for (iter = this->modelMsgs.begin();
        iter != this->modelMsgs.end(); ++iter)
   {
 void World::ProcessFactoryMsgs()
 {
   std::list<msgs::Factory>::iterator iter;
-  boost::mutex::scoped_lock lock(*this->receiveMutex);
 
   for (iter = this->factoryMsgs.begin();
        iter != this->factoryMsgs.end(); ++iter)
 //////////////////////////////////////////////////
 void World::InsertModelFile(const std::string &_sdfFilename)
 {
-  boost::mutex::scoped_lock lock(*this->receiveMutex);
+  boost::recursive_mutex::scoped_lock lock(*this->receiveMutex);
   msgs::Factory msg;
   msg.set_sdf_filename(_sdfFilename);
   this->factoryMsgs.push_back(msg);
 //////////////////////////////////////////////////
 void World::InsertModelSDF(const sdf::SDF &_sdf)
 {
-  boost::mutex::scoped_lock lock(*this->receiveMutex);
+  boost::recursive_mutex::scoped_lock lock(*this->receiveMutex);
   msgs::Factory msg;
   msg.set_sdf(_sdf.ToString());
   this->factoryMsgs.push_back(msg);
 //////////////////////////////////////////////////
 void World::InsertModelString(const std::string &_sdfString)
 {
-  boost::mutex::scoped_lock lock(*this->receiveMutex);
+  boost::recursive_mutex::scoped_lock lock(*this->receiveMutex);
   msgs::Factory msg;
   msg.set_sdf(_sdfString);
   this->factoryMsgs.push_back(msg);
 void World::UpdateStateSDF()
 {
   this->sdf->Update();
-  /*sdf::ElementPtr stateElem = this->sdf->GetElement("state");
+  sdf::ElementPtr stateElem = this->sdf->GetElement("state");
   stateElem->ClearElements();
 
-  stateElem->GetAttribute("world_name")->Set(this->GetName());
-  stateElem->GetElement("time")->Set(this->GetSimTime());
-
-  for (unsigned int i = 0; i < this->GetModelCount(); ++i)
-  {
-    sdf::ElementPtr elem = stateElem->AddElement("model");
-    this->GetModel(i)->GetState().FillStateSDF(elem);
-  }
-  */
+  this->prevStates[(this->stateToggle + 1) % 2].FillSDF(stateElem);
 }
 
 //////////////////////////////////////////////////
 //////////////////////////////////////////////////
 void World::ProcessMessages()
 {
+  boost::recursive_mutex::scoped_lock lock(*this->receiveMutex);
+
+  if (this->posePub && this->posePub->HasConnections() &&
+      this->poseMsgs.pose_size() > 0)
+  {
+    this->posePub->Publish(this->poseMsgs);
+  }
+  this->poseMsgs.clear_pose();
+
   if (common::Time::GetWallTime() - this->prevProcessMsgsTime >
       this->processMsgsPeriod)
   {
   this->statPub->Publish(this->worldStatsMsg);
   this->prevStatTime = common::Time::GetWallTime();
 }
+
+//////////////////////////////////////////////////
+bool World::IsLoaded() const
+{
+  return this->loaded;
+}
+
+//////////////////////////////////////////////////
+void World::EnqueueMsg(msgs::Pose *_msg)
+{
+  if (!_msg)
+    return;
+
+  boost::recursive_mutex::scoped_lock lock(*this->receiveMutex);
+  int i = 0;
+
+  // Replace old pose messages with the new one.
+  for (; i < this->poseMsgs.pose_size(); ++i)
+  {
+    if (this->poseMsgs.pose(i).name() == _msg->name())
+    {
+      this->poseMsgs.mutable_pose(i)->CopyFrom(*_msg);
+      break;
+    }
+  }
+
+  // Add the pose message if no old pose messages were found.
+  if (i >= this->poseMsgs.pose_size())
+      this->poseMsgs.add_pose()->CopyFrom(*_msg);
+}

gazebo/physics/World.hh

       /// \return A pointer to the entity, or NULL if no entity was found.
       public: BasePtr GetByName(const std::string &_name);
 
-      /// \brief Deprecated
-      public: ModelPtr GetModelByName(const std::string &name)GAZEBO_DEPRECATED;
-
       /// \brief Get a model by name.
       ///
       /// This function is the same as GetByName, but limits the search to
       /// \return A pointer to the Model, or NULL if no model was found.
       public: ModelPtr GetModel(const std::string &_name);
 
-      /// \brief Deprecated
-      public: EntityPtr GetEntityByName(
-                  const std::string &_name) GAZEBO_DEPRECATED;
-
       /// \brief Get a pointer to an Entity based on a name.
       ///
       /// This function is the same as GetByName, but limits the search to
       /// \brief Update the state SDF value from the current state.
       public: void UpdateStateSDF();