Commits

Steven Peters  committed 7255e89 Merge

merge with default

  • Participants
  • Parent commits 153ae4a, 5075c0c
  • Branches issue_236

Comments (0)

Files changed (72)

 6cefc0062eaf56a3f414d2d2e0cde897be75b840 gazebo-prerelease_1.3.0
 dc47d59da98b573859681e458e0961b0863ebf25 gazebo-prerelease_1.3.0
 ca149ddc5402996433830057cb7b4a605a636383 gazebo-prerelease_1.3.0
+e93d70fe8d38c68fb48c53f35a74f4b17be7c750 gazebo_1.3.1
+cf5483c2183e0d19342466a8f2ab0050e0407a1c gazebo-prerelease_1.3.1
+cf5483c2183e0d19342466a8f2ab0050e0407a1c gazebo-prerelease_1.3.1
+6aeec94a2cf66f175400b25c00a4b1a64ddee18a gazebo-prerelease_1.3.1
+6aeec94a2cf66f175400b25c00a4b1a64ddee18a gazebo-prerelease_1.3.1
+a7d1ab02e2c32f453eb13f9ff0bb0b0cbda822f3 gazebo-prerelease_1.3.1
+a7d1ab02e2c32f453eb13f9ff0bb0b0cbda822f3 gazebo-prerelease_1.3.1
+ee535bb56127ad041893ed0eff4377829212d1c1 gazebo-prerelease_1.3.1
+ee535bb56127ad041893ed0eff4377829212d1c1 gazebo-prerelease_1.3.1
+33d7c60fdaea2354301274c88d5aee7dbad4239b gazebo-prerelease_1.3.1
+33d7c60fdaea2354301274c88d5aee7dbad4239b gazebo-prerelease_1.3.1
+7b725239d23e4cb387572c4cba02c2eec5a490b2 gazebo-prerelease_1.3.1
+7b725239d23e4cb387572c4cba02c2eec5a490b2 gazebo-prerelease_1.3.1
+f04fd5401a543425d53eddd5d7fda331107d4be8 gazebo-prerelease_1.3.1

File CMakeLists.txt

 
 set (GAZEBO_VERSION_NAME "lithium")
 set (GAZEBO_MAJOR_VERSION 1)
-set (GAZEBO_MINOR_VERSION 3)
+set (GAZEBO_MINOR_VERSION 4)
 set (GAZEBO_PATCH_VERSION 0)
 
 set (GAZEBO_VERSION ${GAZEBO_MAJOR_VERSION}.${GAZEBO_MINOR_VERSION})

File gazebo/gui/MainWindow.cc

 }
 
 /////////////////////////////////////////////////
-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);

File 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

File gazebo/gui/images/copy_object.png

Added
New image

File gazebo/gui/images/paste_object.png

Added
New image

File gazebo/gui/images/rotate_object.png

Added
New image

File gazebo/gui/images/scale_object.png

Added
New image

File 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>

File gazebo/gui/style.qss

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

File 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;

File 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

File 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?

File 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;
     }
 

File gazebo/math/Vector3.hh

 #include <iostream>
 #include <fstream>
 
+#include "gazebo/math/Helpers.hh"
 #include "gazebo/common/CommonTypes.hh"
 
 namespace gazebo
       /// \brief Multiplication operators
       /// \param[in] _v the scaling factor
       /// \return a scaled vector
+      public: friend inline Vector3 operator*(double _s,
+                                              const Vector3 &_v)
+      { return Vector3(_v.x * _s, _v.y * _s, _v.z * _s); }
+
+      /// \brief Multiplication operators
+      /// \param[in] _v the scaling factor
+      /// \return a scaled vector
       public: Vector3 operator*(double _v) const;
 
       /// \brief Multiplication operator
       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;
       }
 

File gazebo/msgs/msgs.cc

           msgs::Set(geomMsg->mutable_mesh()->mutable_scale(),
               geomElem->GetValueVector3("scale"));
 
-          // The if clause is used to detect instances of "filename" with
-          // the sdf. Eventually this code will be deprecated as people
-          // switch to using uris.
-          if (geomElem->GetValueString("filename") != "__default__")
-          {
-            geomMsg->mutable_mesh()->set_filename(
-                common::find_file(geomElem->GetValueString("filename")));
-          }
-          else
-          {
-            geomMsg->mutable_mesh()->set_filename(
-                geomElem->GetValueString("uri"));
-          }
+          geomMsg->mutable_mesh()->set_filename(
+              geomElem->GetValueString("uri"));
         }
         else if (geomElem->GetName() == "empty")
         {

File 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));
-}

File 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;
     };
     /// \}
   }

File 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());

File 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);

File 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);
+}
+

File 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

File gazebo/physics/Contact.cc

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

File 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

File 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);
-}

File 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;
     };
     /// \}
   }

File gazebo/physics/Entity.cc

 //
 void Entity::SetWorldPose(const math::Pose &_pose, bool _notify, bool _publish)
 {
-  boost::mutex::scoped_lock lock(*this->GetWorld()->GetSetWorldPoseMutex());
-
-  math::Pose oldPose = this->worldPose;
-  (*this.*setWorldPoseFunc)(_pose, _notify, _publish);
-
+  {
+    boost::mutex::scoped_lock lock(*this->GetWorld()->GetSetWorldPoseMutex());
+    (*this.*setWorldPoseFunc)(_pose, _notify, _publish);
+  }
   if (_publish)
     this->PublishPose();
 }

File gazebo/physics/Gripper.cc

 
   sdf::ElementPtr grasp_check = _sdf->GetElement("grasp_check");
   this->min_contact_count = grasp_check->GetValueUInt("min_contact_count");
-  this->attachSteps = grasp_check->GetValueInt("attachSteps");
-  this->detachSteps = grasp_check->GetValueInt("detachSteps");
+  this->attachSteps = grasp_check->GetValueInt("attach_steps");
+  this->detachSteps = grasp_check->GetValueInt("detach_steps");
 
   sdf::ElementPtr palmLinkElem = _sdf->GetElement("palm_link");
   this->palmLink = this->model->GetLink(palmLinkElem->GetValueString());
       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::Pose());
           this->fixedJoint->Init();
           this->fixedJoint->SetHighStop(0, 0);
           this->fixedJoint->SetLowStop(0, 0);

File gazebo/physics/Joint.cc

 //////////////////////////////////////////////////
 void Joint::Load(LinkPtr _parent, LinkPtr _child, const math::Vector3 &_pos)
 {
+  this->Load(_parent, _child, math::Pose(_pos, math::Quaternion()));
+}
+
+//////////////////////////////////////////////////
+void Joint::Load(LinkPtr _parent, LinkPtr _child, const math::Pose &_pose)
+{
   if (_parent)
   {
     this->world = _parent->GetWorld();
 
   this->parentLink = _parent;
   this->childLink = _child;
-  this->anchorPos = _pos;
-}
-
-//////////////////////////////////////////////////
-void Joint::Load(LinkPtr _parent, LinkPtr _child, const math::Pose &_pose)
-{
-  this->Load(_parent, _child, _pose.pos);
+  this->LoadImpl(_pose);
 }
 
 //////////////////////////////////////////////////
   if (!this->childLink && childName != std::string("world"))
     gzthrow("Couldn't Find Child Link[" + childName  + "]");
 
-  /// \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->LoadImpl( _sdf->GetValuePose("pose"));
+/////////////////////////////////////////////////
+void Joint::LoadImpl(const math::Vector3 &_pos)
+{
+  this->LoadImpl(math::Pose(_pos, math::Quaternion()));
 }
 
 /////////////////////////////////////////////////
     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->childLink->GetWorldPose().pos);
+  this->SetAnchor(0, this->anchorPos);
 
   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());

File gazebo/physics/Joint.hh

       /// \param[in] _child Child link.
       /// \param[in] _pose Pose containing Joint Anchor offset from child link.
       public: void Load(LinkPtr _parent, LinkPtr _child,
-                        const math::Pose &_pose) GAZEBO_DEPRECATED;
+                        const math::Pose &_pose);
 
       /// \brief Set parent and child links of a physics::Joint and its
       /// anchor offset position.
+      /// This funciton is deprecated, use
+      /// Load(LinkPtr _parent, LinkPtr _child, const math::Pose &_pose)
       /// \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);
+                        const math::Vector3 &_pos); GAZEBO_DEPRECATED;
 
       /// \brief Load physics::Joint from a SDF sdf::Element.
       /// \param[in] _sdf SDF values to load from.
       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) GAZEBO_DEPRECATED;
+      private: void LoadImpl(const math::Pose &_pose);
+
+      /// \brief Helper function to load a joint.
+      /// This function is deprecated, use LoadImpl(math::Pose &)
+      /// \param[in] _pos Position of the anchor.
+      private: void LoadImpl(const math::Vector3 &_pos) GAZEBO_DEPRECATED;
 
       /// \brief The first link this joint connects to
       protected: LinkPtr childLink;

File gazebo/physics/JointController.cc

 /////////////////////////////////////////////////
 void JointController::Reset()
 {
+  // Reset setpoints and feed-forward.
   this->positions.clear();
   this->velocities.clear();
   this->forces.clear();
+  // Should the PID's be reset as well?
 }
 
 /////////////////////////////////////////////////
   common::Time stepTime = currTime - this->prevUpdateTime;
   this->prevUpdateTime = currTime;
 
-  if (this->forces.size() > 0)
+  // Skip the update step if SimTime appears to have gone backward.
+  // Negative update time wreaks havok on the integrators.
+  // This happens when World::ResetTime is called.
+  // TODO: fix this when World::ResetTime is improved
+  if (stepTime > 0)
   {
-    std::map<std::string, double>::iterator iter;
-    for (iter = this->forces.begin(); iter != this->forces.end(); ++iter)
-      this->joints[iter->first]->SetForce(0, iter->second);
-  }
+    if (this->forces.size() > 0)
+    {
+      std::map<std::string, double>::iterator iter;
+      for (iter = this->forces.begin(); iter != this->forces.end(); ++iter)
+        this->joints[iter->first]->SetForce(0, iter->second);
+    }
 
-  if (this->positions.size() > 0)
-  {
-    double cmd;
-    std::map<std::string, double>::iterator iter;
+    if (this->positions.size() > 0)
+    {
+      double cmd;
+      std::map<std::string, double>::iterator iter;
 
-    for (iter = this->positions.begin(); iter != this->positions.end(); ++iter)
+      for (iter = this->positions.begin(); iter != this->positions.end();
+           ++iter)
+      {
+        cmd = this->posPids[iter->first].Update(
+            this->joints[iter->first]->GetAngle(0).Radian() - iter->second,
+            stepTime);
+        this->joints[iter->first]->SetForce(0, cmd);
+      }
+    }
+
+    if (this->velocities.size() > 0)
     {
-      cmd = this->posPids[iter->first].Update(
-          this->joints[iter->first]->GetAngle(0).Radian() - iter->second,
-          stepTime);
-      this->joints[iter->first]->SetForce(0, cmd);
-    }
-  }
+      double cmd;
+      std::map<std::string, double>::iterator iter;
 
-  if (this->velocities.size() > 0)
-  {
-    double cmd;
-    std::map<std::string, double>::iterator iter;
-
-    for (iter = this->velocities.begin();
-         iter != this->velocities.end(); ++iter)
-    {
-      cmd = this->velPids[iter->first].Update(
-          this->joints[iter->first]->GetVelocity(0) - iter->second,
-          stepTime);
-      this->joints[iter->first]->SetForce(0, cmd);
+      for (iter = this->velocities.begin();
+           iter != this->velocities.end(); ++iter)
+      {
+        cmd = this->velPids[iter->first].Update(
+            this->joints[iter->first]->GetVelocity(0) - iter->second,
+            stepTime);
+        this->joints[iter->first]->SetForce(0, cmd);
+      }
     }
   }
 

File 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());
+  }
+}

File 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

File gazebo/physics/Link.cc

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

File 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);

File 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);
+  }
+}

File 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

File gazebo/physics/Model.cc

 #include <boost/thread/recursive_mutex.hpp>
 #include <sstream>
 
-#include "common/KeyFrame.hh"
-#include "common/Animation.hh"
-#include "common/Plugin.hh"
-#include "common/Events.hh"
-#include "common/Exception.hh"
-#include "common/Console.hh"
-#include "common/CommonTypes.hh"
+#include "gazebo/common/KeyFrame.hh"
+#include "gazebo/common/Animation.hh"
+#include "gazebo/common/Plugin.hh"
+#include "gazebo/common/Events.hh"
+#include "gazebo/common/Exception.hh"
+#include "gazebo/common/Console.hh"
+#include "gazebo/common/CommonTypes.hh"
 
-#include "physics/Gripper.hh"
-#include "physics/Joint.hh"
-#include "physics/JointController.hh"
-#include "physics/Link.hh"
-#include "physics/World.hh"
-#include "physics/PhysicsEngine.hh"
-#include "physics/Model.hh"
-#include "physics/Contact.hh"
+#include "gazebo/physics/Gripper.hh"
+#include "gazebo/physics/Joint.hh"
+#include "gazebo/physics/JointController.hh"
+#include "gazebo/physics/Link.hh"
+#include "gazebo/physics/World.hh"
+#include "gazebo/physics/PhysicsEngine.hh"
+#include "gazebo/physics/Model.hh"
+#include "gazebo/physics/Contact.hh"
+
+#include "gazebo/sensors/SensorManager.hh"
 
 #include "transport/Node.hh"
 
 {
   this->AddType(MODEL);
   this->updateMutex = new boost::recursive_mutex();
-  this->jointController = NULL;
 }
 
 //////////////////////////////////////////////////
 Model::~Model()
 {
   delete this->updateMutex;
-  delete this->jointController;
 }
 
 //////////////////////////////////////////////////
       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;
   this->joints.push_back(joint);
 
   if (!this->jointController)
-    this->jointController = new JointController(
-        boost::shared_dynamic_cast<Model>(shared_from_this()));
+    this->jointController.reset(new JointController(
+        boost::shared_dynamic_cast<Model>(shared_from_this())));
   this->jointController->AddJoint(joint);
 }
 
 //////////////////////////////////////////////////
 void Model::LoadPlugins()
 {
-  // Load the plugins
-  if (this->sdf->HasElement("plugin"))
+  // Check to see if we need to load any model plugins
+  if (this->GetPluginCount() > 0)
   {
-    sdf::ElementPtr pluginElem = this->sdf->GetElement("plugin");
-    while (pluginElem)
+    int iterations = 0;
+
+    // Wait for the sensors to be initialized before loading
+    // plugins, if there are any sensors
+    while (this->GetSensorCount() > 0 &&
+        !sensors::SensorManager::Instance()->SensorsInitialized() &&
+        iterations < 50)
     {
-      this->LoadPlugin(pluginElem);
-      pluginElem = pluginElem->GetNextElement("plugin");
+      common::Time::MSleep(100);
+      iterations++;
+    }
+
+    // Load the plugins if the sensors have been loaded, or if there
+    // are no sensors attached to the model.
+    if (iterations < 50)
+    {
+      // Load the plugins
+      sdf::ElementPtr pluginElem = this->sdf->GetElement("plugin");
+      while (pluginElem)
+      {
+        this->LoadPlugin(pluginElem);
+        pluginElem = pluginElem->GetNextElement("plugin");
+      }
+    }
+    else
+    {
+      gzerr << "Sensors failed to initialize when loading model["
+        << this->GetName() << "] via the factory mechanism."
+        << "Plugins for the model will not be loaded.\n";
     }
   }
 }
 }
 
 //////////////////////////////////////////////////
-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";
-      */
   }
 }
 

File gazebo/physics/Model.hh

 {
   namespace physics
   {
-    class JointController;
     class Gripper;
 
     /// \addtogroup gazebo_physics
       /// \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);
       /// \param[in] _sdf SDF parameter.
       private: void LoadGripper(sdf::ElementPtr _sdf);
 
+      /// \brief Get a handle to the Controller for the joints in this model.
+      /// \return A handle to the Controller for the joints in this model.
+      public: JointControllerPtr GetJointController()
+        { return this->jointController; }
+
       /// used by Model::AttachStaticModel
       protected: std::vector<ModelPtr> attachedModels;
 
       private: boost::recursive_mutex *updateMutex;
 
       /// \brief Controller for the joints.
-      private: JointController *jointController;
+      private: JointControllerPtr jointController;
 
       private: bool pluginsLoaded;
     };

File 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);
+  }
+}

File 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

File gazebo/physics/PhysicsTypes.hh

     class Link;
     class Collision;
     class Joint;
+    class JointController;
     class Contact;
     class PhysicsEngine;
     class Mass;
     /// \brief Boost shared pointer to a Joint object
     typedef boost::shared_ptr<Joint> JointPtr;
 
+    /// \def JointControllerPtr
+    /// \brief Boost shared pointer to a JointController object
+    typedef boost::shared_ptr<JointController> JointControllerPtr;
+
     /// \def  PhysicsEnginePtr
     /// \brief Boost shared pointer to a PhysicsEngine object
     typedef boost::shared_ptr<PhysicsEngine> PhysicsEnginePtr;
     /// \brief Vector of JointPtr
     typedef std::vector<JointPtr> Joint_V;
 
+    /// \def JointController_V
+    /// \brief Vector of JointControllerPtr
+    typedef std::vector<JointControllerPtr> JointController_V;
+
     /// \def Link_V
     /// \brief Vector of LinkPtr
     typedef std::vector<LinkPtr>  Link_V;

File 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);
-}

File 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;

File 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);
-}

File 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;
     };
     /// \}
   }

File 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);

File 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);

File 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;
-}

File 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;
     };

File gazebo/physics/World.cc

   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->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)
 {
 //////////////////////////////////////////////////
 void World::Step()
 {
+  /// need this because ODE does not call dxReallocateWorldProcessContext()
+  /// until dWorld.*Step
+  /// Plugins that manipulate joints (and probably other properties) require
+  /// one iteration of the physics engine. Do not remove this.
+  if (!this->pluginsLoaded &&
+      sensors::SensorManager::Instance()->SensorsInitialized())
+  {
+    this->LoadPlugins();
+    this->pluginsLoaded = true;
+  }
+
   // Send statistics about the world simulation
   if (common::Time::GetWallTime() - this->prevStatTime > this->statPeriod)
   {
   // 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();
   }
 }
 
     // This must be called directly after PhysicsEngine::UpdateCollision.
     this->physicsEngine->UpdatePhysics();
 
-    /// need this because ODE does not call dxReallocateWorldProcessContext()
-    /// until dWorld.*Step
-    /// Plugins that manipulate joints (and probably other properties) require
-    /// one iteration of the physics engine. Do not remove this.
-    if (!this->pluginsLoaded &&
-        sensors::SensorManager::Instance()->SensorsInitialized())
-    {
-      this->LoadPlugins();
-      this->pluginsLoaded = true;
-    }
-
     // do this after physics update as
     //   ode --> MoveCallback sets the dirtyPoses
     //           and we need to propagate it into Entity::worldPose
 {
   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();
     {
       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);
         ModelPtr model = this->LoadModel(elem, this->rootElement);
         model->Init();
 
-        // Check to see if we need to load any model plugins
-        if (model->GetPluginCount() > 0)
-        {
-          int iterations = 0;
-
-          // Wait for the sensors to be initialized before loading
-          // plugins, if there are any sensors
-          while (model->GetSensorCount() > 0 &&
-              !sensors::SensorManager::Instance()->SensorsInitialized() &&
-              iterations < 50)
-          {
-            common::Time::MSleep(100);
-            iterations++;
-          }
-
-          // Load the plugins if the sensors have been loaded, or if there
-          // are no sensors attached to the model.
-          if (iterations < 50)
-            model->LoadPlugins();
-          else
-          {
-            gzerr << "Sensors failed to initialize when loading model["
-              << model->GetName() << "] via the factory mechanism."
-              << "Plugins for the model will not be loaded.\n";
-          }
-        }
+        model->LoadPlugins();
       }
       else if (isLight)
       {
 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);
 }
 
 //////////////////////////////////////////////////
 }
 
 //////////////////////////////////////////////////
+bool World::IsLoaded() const
+{
+  return this->loaded;
+}
+
+//////////////////////////////////////////////////
 void World::EnqueueMsg(msgs::Pose *_msg)
 {
   if (!_msg)

File 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();
 
+      /// \brief Return true if the world has been loaded.
+      /// \return True if World::Load has completed.
+      public: bool IsLoaded() const;
+
       /// \brief Enqueue a pose message for publication.
       /// These messages will be transmitted at the end of every iteration.
       /// \param[in] _msg The message to enqueue.
       /// \brief True if the world has been initialized.
       private: bool initialized;
 
+      /// \brief True if the world has been loaded.
+      private: bool loaded;
+
       /// \brief True to enable the physics engine.
       private: bool enablePhysicsEngine;
 

File gazebo/physics/WorldState.cc

 
   return result;
 }
+
+/////////////////////////////////////////////////
+void WorldState::FillSDF(sdf::ElementPtr _sdf)
+{
+  _sdf->ClearElements();
+
+  _sdf->GetAttribute("world_name")->Set(this->name);
+  _sdf->GetElement("sim_time")->Set(this->simTime);
+  _sdf->GetElement("real_time")->Set(this->realTime);
+  _sdf->GetElement("wall_time")->Set(this->wallTime);
+
+  for (std::vector<ModelState>::iterator iter =
+       this->modelStates.begin(); iter != this->modelStates.end(); ++iter)
+  {
+    sdf::ElementPtr elem = _sdf->AddElement("model");
+    (*iter).FillSDF(elem);
+  }
+}

File gazebo/physics/WorldState.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

File gazebo/physics/ode/ODEJoint.cc

   // f2, t2 are the force torque measured on parent body's cg
   // f1, t1 are the force torque measured on child body's cg
   dJointFeedback* fb = this->GetFeedback();
-  wrench.body1Force.Set(fb->f1[0], fb->f1[1], fb->f1[2]);
-  wrench.body1Torque.Set(fb->t1[0], fb->t1[1], fb->t1[2]);
-  wrench.body2Force.Set(fb->f2[0], fb->f2[1], fb->f2[2]);
-  wrench.body2Torque.Set(fb->t2[0], fb->t2[1], fb->t2[2]);
+  if (fb)
+  {
+    wrench.body1Force.Set(fb->f1[0], fb->f1[1], fb->f1[2]);
+    wrench.body1Torque.Set(fb->t1[0], fb->t1[1], fb->t1[2]);
+    wrench.body2Force.Set(fb->f2[0], fb->f2[1], fb->f2[2]);
+    wrench.body2Torque.Set(fb->t2[0], fb->t2[1], fb->t2[2]);
 
-  {
-    // convert torque from about child CG to joint anchor location
-    // cg position specified in child link frame
-    math::Vector3 cgPos = this->childLink->GetInertial()->GetPose().pos;
-    // moment arm rotated into world frame (given feedback is in world frame)
-    math::Vector3 childMomentArm =
-      this->childLink->GetWorldPose().rot.RotateVector(
-      this->anchorPos - cgPos);
+    if (this->childLink)
+    {
+      // convert torque from about child CG to joint anchor location
+      // cg position specified in child link frame
+      math::Vector3 cgPos = this->childLink->GetInertial()->GetPose().pos;
+      // moment arm rotated into world frame (given feedback is in world frame)
+      math::Vector3 childMomentArm =
+        this->childLink->GetWorldPose().rot.RotateVector(
+        this->anchorPos - cgPos);
 
-    // gzerr << "anchor [" << anchorPos