Commits

Ian Chen committed 3338205 Merge

Merge from default

Comments (0)

Files changed (34)

gazebo/Server.cc

File contents unchanged.

gazebo/gui/ModelListWidget.cc

     }
   }
 
-  this->modelPub->Publish(msg);
+  // \todo Renable when modifying a model is fixed.
+  // this->modelPub->Publish(msg);
+  gzwarn << "Model modification is currently disabled. "
+         << "Look for this feature in Gazebo 2.0\n";
 }
 
 /////////////////////////////////////////////////

gazebo/physics/Collision.cc

File contents unchanged.

gazebo/physics/Collision.hh

       public: virtual void UpdateParameters(sdf::ElementPtr _sdf);
 
       /// \brief Set the encapsulated collsion object.
-      /// \param[in] _placeable True to make the object m.
+      /// \param[in] _placeable True to make the object movable.
       public: void SetCollision(bool _placeable);
 
       /// \brief Return whether this collision is movable.

gazebo/physics/Joint.cc

  * Date: 21 May 2003
  */
 
-#include "transport/Transport.hh"
-#include "transport/Publisher.hh"
+#include "gazebo/transport/Transport.hh"
+#include "gazebo/transport/Publisher.hh"
 
-#include "common/Events.hh"
-#include "common/Exception.hh"
-#include "common/Console.hh"
+#include "gazebo/common/Assert.hh"
+#include "gazebo/common/Console.hh"
+#include "gazebo/common/Events.hh"
+#include "gazebo/common/Exception.hh"
 
-#include "physics/PhysicsEngine.hh"
-#include "physics/Link.hh"
-#include "physics/Model.hh"
-#include "physics/World.hh"
-#include "physics/Joint.hh"
+#include "gazebo/physics/PhysicsEngine.hh"
+#include "gazebo/physics/Link.hh"
+#include "gazebo/physics/Model.hh"
+#include "gazebo/physics/World.hh"
+#include "gazebo/physics/Joint.hh"
 
 using namespace gazebo;
 using namespace physics;
 
   this->parentLink = _parent;
   this->childLink = _child;
+
+  // Joint is loaded without sdf from a model
+  // Initialize this->sdf so it can be used for data storage
+  sdf::initFile("joint.sdf", this->sdf);
+
   this->LoadImpl(_pose);
 }
 
 
   if (this->parentLink)
     this->parentLink->AddChildJoint(boost::shared_static_cast<Joint>(myBase));
-  this->childLink->AddParentJoint(boost::shared_static_cast<Joint>(myBase));
+  else if (this->childLink)
+    this->childLink->AddParentJoint(boost::shared_static_cast<Joint>(myBase));
+  else
+    gzthrow("both parent and child link do no exist");
 
-  // setting anchor relative to gazebo link frame pose
+  // setting anchor relative to gazebo child link frame position
   if (this->childLink)
     this->anchorPos = (_pose + this->childLink->GetWorldPose()).pos;
+  // otherwise set anchor relative to world frame
   else
-    this->anchorPos = math::Vector3(0, 0, 0);
+    this->anchorPos = _pose.pos;
 }
 
 //////////////////////////////////////////////////
 }
 
 //////////////////////////////////////////////////
+void Joint::SetHighStop(int _index, const math::Angle &_angle)
+{
+  GZ_ASSERT(this->sdf != NULL, "Joint sdf member is NULL");
+  if (_index == 0)
+  {
+    this->sdf->GetElement("axis")->GetElement("limit")
+             ->GetElement("upper")->Set(_angle.Radian());
+  }
+  else if (_index == 1)
+  {
+    this->sdf->GetElement("axis2")->GetElement("limit")
+             ->GetElement("upper")->Set(_angle.Radian());
+  }
+  else
+  {
+    gzerr << "Invalid joint index [" << _index
+          << "] when trying to set high stop\n";
+  }
+}
+
+//////////////////////////////////////////////////
+void Joint::SetLowStop(int _index, const math::Angle &_angle)
+{
+  GZ_ASSERT(this->sdf != NULL, "Joint sdf member is NULL");
+  if (_index == 0)
+  {
+    this->sdf->GetElement("axis")->GetElement("limit")
+             ->GetElement("lower")->Set(_angle.Radian());
+  }
+  else if (_index == 1)
+  {
+    this->sdf->GetElement("axis2")->GetElement("limit")
+             ->GetElement("lower")->Set(_angle.Radian());
+  }
+  else
+  {
+    gzerr << "Invalid joint index [" << _index
+          << "] when trying to set low stop\n";
+  }
+}
+
+//////////////////////////////////////////////////
 void Joint::SetAngle(int /*index*/, math::Angle _angle)
 {
   if (this->model->IsStatic())

gazebo/physics/Joint.hh

 
       /// \brief Set the axis of rotation.
       /// \param[in] _index Index of the axis to set.
-      /// \param[in] _axis Axis value.
+      /// \param[in] _axis Vector in world frame of axis direction
+      ///                  (must have length greater than zero).
       public: virtual void SetAxis(int _index, const math::Vector3 &_axis) = 0;
 
       /// \brief Set the joint damping.
       /// \param[in] _index Index of the axis.
       /// \param[in] _angle High stop angle.
       public: virtual void SetHighStop(int _index,
-                                       const math::Angle &_angle) = 0;
+                                       const math::Angle &_angle);
 
       /// \brief Set the low stop of an axis(index).
       /// \param[in] _index Index of the axis.
       /// \param[in] _angle Low stop angle.
       public: virtual void SetLowStop(int _index,
-                                      const math::Angle &_angle) = 0;
+                                      const math::Angle &_angle);
 
       /// \brief Get the high stop of an axis(index).
       /// \param[in] _index Index of the axis.
       /// \brief Pointer to the parent model.
       protected: ModelPtr model;
 
-      /// \brief Anchor pose.
+      /// \brief Anchor position of joint, expressed in world frame.
       protected: math::Vector3 anchorPos;
 
-      /// \brief Anchor link.
-      protected: LinkPtr anchorLink;
-
       /// \brief Joint update event.
       private: event::EventT<void ()> jointUpdate;
 

gazebo/physics/Model.cc

        iter != this->joints.end(); ++iter)
   {
     (*iter)->Init();
+    // The following message used to be filled and sent in Model::LoadJoint
+    // It is moved here, after Joint::Init, so that the joint properties
+    // can be included in the message.
+    msgs::Joint msg;
+    (*iter)->FillMsg(msg);
+    this->jointPub->Publish(msg);
   }
 
   for (std::vector<Gripper*>::iterator iter = this->grippers.begin();
   if (this->GetJoint(joint->GetScopedName()) != NULL)
     gzthrow("can't have two joint with the same name");
 
-  msgs::Joint msg;
-  joint->FillMsg(msg);
-  this->jointPub->Publish(msg);
-
   this->joints.push_back(joint);
 
   if (!this->jointController)
 {
   return this->sdf->GetValueBool("allow_auto_disable");
 }
-

gazebo/physics/PhysicsTypes.hh

 
     /// \def GZ_SENSOR_COLLIDE
     /// \brief Collision object will collide only with sensors
-    #define GZ_SENSOR_COLLIDE 0x00000003
+    #define GZ_SENSOR_COLLIDE 0x00000002
 
     /// \def GZ_GHOST_COLLIDE
     /// \brief Collides with everything else but other ghost.

gazebo/physics/World.cc

File contents unchanged.

gazebo/physics/bullet/BulletBallJoint.cc

  * Date: 21 May 2003
  */
 
-#include "common/Exception.hh"
-#include "common/Console.hh"
+#include "gazebo/common/Assert.hh"
+#include "gazebo/common/Console.hh"
+#include "gazebo/common/Exception.hh"
 
-#include "physics/bullet/BulletTypes.hh"
-#include "physics/bullet/BulletLink.hh"
-#include "physics/bullet/BulletBallJoint.hh"
+#include "gazebo/physics/bullet/BulletTypes.hh"
+#include "gazebo/physics/bullet/BulletLink.hh"
+#include "gazebo/physics/bullet/BulletBallJoint.hh"
 
 using namespace gazebo;
 using namespace physics;
 BulletBallJoint::BulletBallJoint(btDynamicsWorld *_world, BasePtr _parent)
     : BallJoint<BulletJoint>(_parent)
 {
-  this->world = _world;
+  GZ_ASSERT(_world, "bullet world pointer is NULL");
+  this->bulletWorld = _world;
   this->bulletBall = NULL;
 }
 
   this->constraint = this->bulletBall;
 
   // Add the joint to the world
-  this->world->addConstraint(this->constraint);
+  GZ_ASSERT(this->bulletWorld, "bullet world pointer is NULL");
+  this->bulletWorld->addConstraint(this->constraint);
 
   // Allows access to impulse
   this->constraint->enableFeedback(true);

gazebo/physics/bullet/BulletCollision.cc

 void BulletCollision::Load(sdf::ElementPtr _sdf)
 {
   Collision::Load(_sdf);
+
+  if (this->IsStatic())
+  {
+    this->SetCategoryBits(GZ_FIXED_COLLIDE);
+    this->SetCollideBits(~GZ_FIXED_COLLIDE);
+  }
 }
 
 //////////////////////////////////////////////////
 }
 
 //////////////////////////////////////////////////
-void BulletCollision::SetCategoryBits(unsigned int /*_bits*/)
+void BulletCollision::SetCategoryBits(unsigned int _bits)
 {
+  this->categoryBits = _bits;
 }
 
 //////////////////////////////////////////////////
-void BulletCollision::SetCollideBits(unsigned int /*_bits*/)
+void BulletCollision::SetCollideBits(unsigned int _bits)
 {
+  this->collideBits = _bits;
+}
+
+//////////////////////////////////////////////////
+unsigned int BulletCollision::GetCategoryBits() const
+{
+  return this->categoryBits;
+}
+
+//////////////////////////////////////////////////
+unsigned int BulletCollision::GetCollideBits() const
+{
+  return this->collideBits;
 }
 
 //////////////////////////////////////////////////
 }
 
 //////////////////////////////////////////////////
-void BulletCollision::SetCollisionShape(btCollisionShape *_shape)
+void BulletCollision::SetCollisionShape(btCollisionShape *_shape,
+    bool _placeable)
 {
+  Collision::SetCollision(_placeable);
   this->collisionShape = _shape;
 
   // btmath::Vector3 vec;

gazebo/physics/bullet/BulletCollision.hh

 
       /// \brief Set the category bits, used during collision detection
       /// \param bits The bits
-      public: virtual void SetCategoryBits(unsigned int bits);
+      public: virtual void SetCategoryBits(unsigned int _bits);
 
       /// \brief Set the collide bits, used during collision detection
       /// \param bits The bits
-      public: virtual void SetCollideBits(unsigned int bits);
+      public: virtual void SetCollideBits(unsigned int _bits);
+
+      /// \brief Get the category bits, used during collision detection
+      /// \return The bits
+      public: virtual unsigned int GetCategoryBits() const;
+
+      /// \brief Get the collide bits, used during collision detection
+      /// \return The bits
+      public: virtual unsigned int GetCollideBits() const;
 
       /// \brief Get the bounding box, defined by the physics engine
       public: virtual math::Box GetBoundingBox() const;
 
       /// \brief Set the collision shape
-      public: void SetCollisionShape(btCollisionShape *shape);
+      /// \param[in] _shape Collision shape
+      /// \param[in] _placeable True to make the object movable.
+      public: void SetCollisionShape(btCollisionShape *_shape,
+          bool _placeable = true);
 
       /// \brief Get the bullet collision shape
       public: btCollisionShape *GetCollisionShape() const;
 
       /// \brief Set the index of the compound shape
-      public: void SetCompoundShapeIndex(int index);
+      public: void SetCompoundShapeIndex(int _index);
 
       protected: btCollisionShape *collisionShape;
+
+      /// \brief Category bits for collision detection
+      private: unsigned int categoryBits;
+
+      /// \brief Collide bits for collision detection
+      private: unsigned int collideBits;
     };
     /// \}
   }

gazebo/physics/bullet/BulletHeightmapShape.cc

   BulletCollisionPtr bParent;
   bParent = boost::shared_dynamic_cast<BulletCollision>(this->collisionParent);
 
-  bParent->SetCollisionShape(this->heightFieldShape);
+  bParent->SetCollisionShape(this->heightFieldShape, false);
 
   math::Pose pose;
   pose.pos.x = 0;

gazebo/physics/bullet/BulletHinge2Joint.cc

  * Date: 21 May 2003
  */
 
-#include "common/Exception.hh"
-#include "common/Console.hh"
+#include "gazebo/common/Assert.hh"
+#include "gazebo/common/Console.hh"
+#include "gazebo/common/Exception.hh"
 
-#include "physics/bullet/BulletTypes.hh"
-#include "physics/bullet/BulletLink.hh"
-#include "physics/bullet/BulletPhysics.hh"
-#include "physics/bullet/BulletHinge2Joint.hh"
+#include "gazebo/physics/bullet/BulletTypes.hh"
+#include "gazebo/physics/bullet/BulletLink.hh"
+#include "gazebo/physics/bullet/BulletPhysics.hh"
+#include "gazebo/physics/bullet/BulletHinge2Joint.hh"
 
 using namespace gazebo;
 using namespace physics;
 BulletHinge2Joint::BulletHinge2Joint(btDynamicsWorld *_world, BasePtr _parent)
     : Hinge2Joint<BulletJoint>(_parent)
 {
-  this->world = _world;
+  GZ_ASSERT(_world, "bullet world pointer is NULL");
+  this->bulletWorld = _world;
   this->bulletHinge2 = NULL;
 }
 
   this->constraint = this->bulletHinge2;
 
   // Add the joint to the world
-  this->world->addConstraint(this->constraint, true);
+  GZ_ASSERT(this->bulletWorld, "bullet world pointer is NULL");
+  this->bulletWorld->addConstraint(this->constraint, true);
 
   // Allows access to impulse
   this->constraint->enableFeedback(true);

gazebo/physics/bullet/BulletHingeJoint.cc

  * Author: Nate Koenig, Andrew Howard
  * Date: 21 May 2003
  */
-#include "common/Console.hh"
-#include "common/Exception.hh"
+#include "gazebo/common/Assert.hh"
+#include "gazebo/common/Console.hh"
+#include "gazebo/common/Exception.hh"
 
-#include "physics/bullet/BulletLink.hh"
-#include "physics/bullet/BulletPhysics.hh"
-#include "physics/bullet/BulletHingeJoint.hh"
+#include "gazebo/physics/bullet/BulletLink.hh"
+#include "gazebo/physics/bullet/BulletPhysics.hh"
+#include "gazebo/physics/bullet/BulletHingeJoint.hh"
 
 using namespace gazebo;
 using namespace physics;
 BulletHingeJoint::BulletHingeJoint(btDynamicsWorld *_world, BasePtr _parent)
     : HingeJoint<BulletJoint>(_parent)
 {
-  this->world = _world;
+  GZ_ASSERT(_world, "bullet world pointer is NULL");
+  this->bulletWorld = _world;
   this->bulletHinge = NULL;
   this->angleOffset = 0;
 }
 }
 
 //////////////////////////////////////////////////
-void BulletHingeJoint::Attach(LinkPtr _one, LinkPtr _two)
+void BulletHingeJoint::Init()
 {
-  HingeJoint<BulletJoint>::Attach(_one, _two);
+  HingeJoint<BulletJoint>::Init();
 
   // Cast to BulletLink
   BulletLinkPtr bulletChildLink =
   BulletLinkPtr bulletParentLink =
     boost::shared_static_cast<BulletLink>(this->parentLink);
 
-  // Get axis from sdf.
-  sdf::ElementPtr axisElem = this->sdf->GetElement("axis");
-  math::Vector3 axis = axisElem->GetValueVector3("xyz");
+  // Get axis unit vector (expressed in world frame).
+  math::Vector3 axis = this->initialWorldAxis;
+  if (axis == math::Vector3::Zero)
+  {
+    gzerr << "axis must have non-zero length, resetting to 0 0 1\n";
+    axis.Set(0, 0, 1);
+  }
 
   // Local variables used to compute pivots and axes in body-fixed frames
   // for the parent and child links.
-  math::Vector3 pivotA, pivotB, axisA, axisB;
+  math::Vector3 pivotParent, pivotChild, axisParent, axisChild;
   math::Pose pose;
 
-  // Initialize pivot[AB] to anchorPos, which is expressed in the
+  // Initialize pivots to anchorPos, which is expressed in the
   // world coordinate frame.
-  pivotA = this->anchorPos;
-  pivotB = this->anchorPos;
+  pivotParent = this->anchorPos;
+  pivotChild = this->anchorPos;
+
   // Check if parentLink exists. If not, the parent will be the world.
   if (this->parentLink)
   {
     // Compute relative pose between joint anchor and CoG of parent link.
     pose = this->parentLink->GetWorldCoGPose();
     // Subtract CoG position from anchor position, both in world frame.
-    pivotA -= pose.pos;
+    pivotParent -= pose.pos;
     // Rotate pivot offset and axis into body-fixed frame of parent.
-    pivotA = pose.rot.RotateVectorReverse(pivotA);
-    axisA = pose.rot.RotateVectorReverse(axis);
+    pivotParent = pose.rot.RotateVectorReverse(pivotParent);
+    axisParent = pose.rot.RotateVectorReverse(axis);
+    axisParent = axisParent.Normalize();
   }
   // Check if childLink exists. If not, the child will be the world.
   if (this->childLink)
     // Compute relative pose between joint anchor and CoG of child link.
     pose = this->childLink->GetWorldCoGPose();
     // Subtract CoG position from anchor position, both in world frame.
-    pivotB -= pose.pos;
+    pivotChild -= pose.pos;
     // Rotate pivot offset and axis into body-fixed frame of child.
-    pivotB = pose.rot.RotateVectorReverse(pivotB);
-    axisB = pose.rot.RotateVectorReverse(axis);
+    pivotChild = pose.rot.RotateVectorReverse(pivotChild);
+    axisChild = pose.rot.RotateVectorReverse(axis);
+    axisChild = axisChild.Normalize();
   }
 
-  axisA = axisA.Round();
-  axisB = axisB.Round();
-
   // If both links exist, then create a joint between the two links.
   if (bulletChildLink && bulletParentLink)
   {
     this->bulletHinge = new btHingeConstraint(
+        *(bulletChildLink->GetBulletLink()),
         *(bulletParentLink->GetBulletLink()),
-        *(bulletChildLink->GetBulletLink()),
-        BulletTypes::ConvertVector3(pivotA),
-        BulletTypes::ConvertVector3(pivotB),
-        BulletTypes::ConvertVector3(axisA),
-        BulletTypes::ConvertVector3(axisB));
+        BulletTypes::ConvertVector3(pivotChild),
+        BulletTypes::ConvertVector3(pivotParent),
+        BulletTypes::ConvertVector3(axisChild),
+        BulletTypes::ConvertVector3(axisParent));
   }
   // If only the child exists, then create a joint between the child
   // and the world.
   {
     this->bulletHinge = new btHingeConstraint(
         *(bulletChildLink->GetBulletLink()),
-        btVector3(pivotB.x, pivotB.y, pivotB.z),
-        btVector3(axisB.x, axisB.y, axisB.z));
+        BulletTypes::ConvertVector3(pivotChild),
+        BulletTypes::ConvertVector3(axisChild));
   }
   // If only the parent exists, then create a joint between the parent
   // and the world.
   {
     this->bulletHinge = new btHingeConstraint(
         *(bulletParentLink->GetBulletLink()),
-        btVector3(pivotA.x, pivotA.y, pivotA.z),
-        btVector3(axisA.x, axisA.y, axisA.z));
+        BulletTypes::ConvertVector3(pivotParent),
+        BulletTypes::ConvertVector3(axisParent));
   }
   // Throw an error if no links are given.
   else
     gzthrow("joint without links\n");
   }
 
+  if (!this->bulletHinge)
+    gzthrow("unable to create bullet hinge constraint\n");
+
   // Give parent class BulletJoint a pointer to this constraint.
   this->constraint = this->bulletHinge;
 
   // GetAngleImpl will report angles relative to this offset.
   this->angleOffset = this->bulletHinge->getHingeAngle();
 
-  // TODO: apply joint limits here.
-  // this->bulletHinge->setLimit(angle - .4, angle + .4);
+  // Apply joint angle limits here.
+  // TODO: velocity and effort limits.
+  GZ_ASSERT(this->sdf != NULL, "Joint sdf member is NULL");
+  sdf::ElementPtr limitElem;
+  limitElem = this->sdf->GetElement("axis")->GetElement("limit");
+  this->bulletHinge->setLimit(
+    this->angleOffset + limitElem->GetValueDouble("lower"),
+    this->angleOffset + limitElem->GetValueDouble("upper"));
 
   // Add the joint to the world
-  this->world->addConstraint(this->bulletHinge, true);
+  GZ_ASSERT(this->bulletWorld, "bullet world pointer is NULL");
+  this->bulletWorld->addConstraint(this->bulletHinge, true);
 
   // Allows access to impulse
   this->bulletHinge->enableFeedback(true);
 }
 
 //////////////////////////////////////////////////
-void BulletHingeJoint::SetAxis(int /*_index*/, const math::Vector3 &/*_axis*/)
+void BulletHingeJoint::SetAxis(int /*_index*/, const math::Vector3 &_axis)
 {
+  // Note that _axis is given in a world frame,
+  // but bullet uses a body-fixed frame
+  if (this->bulletHinge == NULL)
+  {
+    // this hasn't been initialized yet, store axis in initialWorldAxis
+    this->initialWorldAxis = _axis;
+  }
+  else
+  {
+    gzerr << "SetAxis for existing joint is not implemented\n";
+  }
+
   // Bullet seems to handle setAxis improperly. It readjust all the pivot
   // points
   /*btmath::Vector3 vec(_axis.x, _axis.y, _axis.z);
 math::Angle BulletHingeJoint::GetAngleImpl(int /*_index*/) const
 {
   math::Angle result;
-  if (this->bulletHinge != NULL)
+  if (this->bulletHinge)
     result = this->bulletHinge->getHingeAngle() - this->angleOffset;
   else
     gzwarn << "bulletHinge does not exist, returning default angle\n";
 //////////////////////////////////////////////////
 double BulletHingeJoint::GetVelocity(int /*_index*/) const
 {
-  gzerr << "Not implemented...\n";
-  return 0;
+  double result = 0;
+  math::Vector3 globalAxis = this->GetGlobalAxis(0);
+  if (this->childLink)
+    result += globalAxis.Dot(this->childLink->GetWorldAngularVel());
+  if (this->parentLink)
+    result -= globalAxis.Dot(this->parentLink->GetWorldAngularVel());
+  return result;
 }
 
 //////////////////////////////////////////////////
     _effort = math::clamp(_effort, -this->effortLimit[_index],
        this->effortLimit[_index]);
 
-  // math::Vector3 axis = this->GetLocalAxis(_index);
-  // this->bulletHinge->enableAngularMotor(true);
+  if (this->bulletHinge)
+  {
+    BulletJoint::SetForce(_index, _effort);
 
-  // z-axis of constraint frame
-  btVector3 hingeAxisLocal =
-    this->bulletHinge->getAFrame().getBasis().getColumn(2);
+    // z-axis of constraint frame
+    btVector3 hingeAxisLocalA =
+      this->bulletHinge->getFrameOffsetA().getBasis().getColumn(2);
+    btVector3 hingeAxisLocalB =
+      this->bulletHinge->getFrameOffsetB().getBasis().getColumn(2);
 
-  btVector3 hingeAxisWorld =
-    this->bulletHinge->getRigidBodyA().getWorldTransform().getBasis() *
-    hingeAxisLocal;
+    btVector3 hingeAxisWorldA =
+      this->bulletHinge->getRigidBodyA().getWorldTransform().getBasis() *
+      hingeAxisLocalA;
+    btVector3 hingeAxisWorldB =
+      this->bulletHinge->getRigidBodyB().getWorldTransform().getBasis() *
+      hingeAxisLocalB;
 
-  btVector3 hingeTorque = _effort * hingeAxisWorld;
+    btVector3 hingeTorqueA = _effort * hingeAxisWorldA;
+    btVector3 hingeTorqueB = _effort * hingeAxisWorldB;
 
-  this->bulletHinge->getRigidBodyA().applyTorque(hingeTorque);
-  this->bulletHinge->getRigidBodyB().applyTorque(-hingeTorque);
-}
-
-//////////////////////////////////////////////////
-double BulletHingeJoint::GetForce(int /*_index*/)
-{
-  return this->bulletHinge->getAppliedImpulse();
+    this->bulletHinge->getRigidBodyA().applyTorque(hingeTorqueA);
+    this->bulletHinge->getRigidBodyB().applyTorque(-hingeTorqueB);
+  }
 }
 
 //////////////////////////////////////////////////
 void BulletHingeJoint::SetHighStop(int /*_index*/,
-                                   const math::Angle &/*_angle*/)
+                      const math::Angle &_angle)
 {
+  Joint::SetHighStop(0, _angle);
   if (this->bulletHinge)
   {
     // this function has additional parameters that we may one day
     // implement. Be warned that this function will reset them to default
     // settings
-    // this->bulletHinge->setLimit(this->bulletHinge->getLowerLimit(),
-    //                         _angle.Radian());
-  }
-  else
-  {
-    gzthrow("Joint must be created first");
+    this->bulletHinge->setLimit(this->bulletHinge->getLowerLimit(),
+                                this->angleOffset + _angle.Radian());
   }
 }
 
 //////////////////////////////////////////////////
 void BulletHingeJoint::SetLowStop(int /*_index*/,
-                                  const math::Angle &/*_angle*/)
+                     const math::Angle &_angle)
 {
+  Joint::SetLowStop(0, _angle);
   if (this->bulletHinge)
   {
     // this function has additional parameters that we may one day
     // implement. Be warned that this function will reset them to default
     // settings
-    // this->bulletHinge->setLimit(-_angle.Radian(),
-    //                         this->bulletHinge->getUpperLimit());
+    this->bulletHinge->setLimit(this->angleOffset + _angle.Radian(),
+                                this->bulletHinge->getUpperLimit());
   }
-  else
-    gzthrow("Joint must be created first");
 }
 
 //////////////////////////////////////////////////
   if (this->bulletHinge)
     result = this->bulletHinge->getUpperLimit();
   else
-    gzthrow("Joint must be created first");
+    gzerr << "Joint must be created before getting high stop\n";
 
   return result;
 }
   if (this->bulletHinge)
     result = this->bulletHinge->getLowerLimit();
   else
-    gzthrow("Joint must be created first");
+    gzerr << "Joint must be created before getting low stop\n";
 
   return result;
 }

gazebo/physics/bullet/BulletHingeJoint.hh

       protected: virtual void Load(sdf::ElementPtr _sdf);
 
       // Documentation inherited.
-      public: virtual void Attach(LinkPtr _one, LinkPtr _two);
+      public: virtual void Init();
 
       // Documentation inherited.
       public: virtual math::Vector3 GetAnchor(int _index) const;
       public: virtual void SetForce(int _index, double _effort);
 
       // Documentation inherited.
-      public: virtual double GetForce(int _index);
-
-      // Documentation inherited.
       public: virtual void SetHighStop(int _index, const math::Angle &_angle);
 
       // Documentation inherited.
       /// \brief Offset angle used in GetAngleImpl, so that angles are reported
       ///        relative to the initial configuration.
       private: double angleOffset;
+
+      /// \brief Initial value of joint axis, expressed as unit vector
+      ///        in world frame.
+      private: math::Vector3 initialWorldAxis;
     };
     /// \}
   }

gazebo/physics/bullet/BulletJoint.cc

   : Joint(_parent)
 {
   this->constraint = NULL;
-  this->world = NULL;
+  this->bulletWorld = NULL;
 }
 
 //////////////////////////////////////////////////
 BulletJoint::~BulletJoint()
 {
   delete this->constraint;
-  this->world = NULL;
+  this->bulletWorld = NULL;
 }
 
 //////////////////////////////////////////////////
 {
   this->childLink.reset();
   this->parentLink.reset();
-
+  if (this->constraint && this->bulletWorld)
+    this->bulletWorld->removeConstraint(this->constraint);
   delete this->constraint;
 }
 

gazebo/physics/bullet/BulletJoint.hh

       public: virtual ~BulletJoint();
 
       /// \brief Load a BulletJoint
-      public: void Load(sdf::ElementPtr _sdf);
+      public: virtual void Load(sdf::ElementPtr _sdf);
 
       /// \brief Reset the joint
       public: virtual void Reset();
               {gzerr << "Not implement in Bullet\n";}
 
       protected: btTypedConstraint *constraint;
-      protected: btDynamicsWorld *world;
+      protected: btDynamicsWorld *bulletWorld;
 
       // Documentation inherited.
       public: virtual JointWrench GetForceTorque(int _index);

gazebo/physics/bullet/BulletLink.cc

 
   btDynamicsWorld *bulletWorld = this->bulletPhysics->GetDynamicsWorld();
   GZ_ASSERT(bulletWorld != NULL, "Bullet dynamics world is NULL");
-  bulletWorld->addRigidBody(this->rigidLink);
+
+  // bullet supports setting bits to a rigid body but not individual
+  // shapes/collisions so find the first child collision and set rigid body to
+  // use its category and collision bits.
+  unsigned int categortyBits = GZ_ALL_COLLIDE;
+  unsigned int collideBits = GZ_ALL_COLLIDE;
+  BulletCollisionPtr collision;
+  for (Base_V::iterator iter = this->children.begin();
+         iter != this->children.end(); ++iter)
+  {
+    if ((*iter)->HasType(Base::COLLISION))
+    {
+      collision = boost::shared_static_cast<BulletCollision>(*iter);
+      categortyBits = collision->GetCategoryBits();
+      collideBits = collision->GetCollideBits();
+      break;
+    }
+  }
+  bulletWorld->addRigidBody(this->rigidLink, categortyBits, collideBits);
   // this->rigidLink->setSleepingThresholds(0,0);
 
   this->SetGravityMode(this->sdf->GetValueBool("gravity"));
 }
 
 //////////////////////////////////////////////////
-void BulletLink::SetSelfCollide(bool /*_collide*/)
+void BulletLink::SetSelfCollide(bool _collide)
 {
+  this->sdf->GetElement("self_collide")->Set(_collide);
 }
 
 //////////////////////////////////////////////////

gazebo/physics/bullet/BulletPhysics.cc

 #include "gazebo/math/Vector3.hh"
 #include "gazebo/math/Rand.hh"
 
-#include "BulletPhysics.hh"
+#include "gazebo/physics/bullet/BulletPhysics.hh"
 
 using namespace gazebo;
 using namespace physics;
 extern ContactProcessedCallback gContactProcessedCallback;
 
 //////////////////////////////////////////////////
+struct CollisionFilter : public btOverlapFilterCallback
+{
+  // return true when pairs need collision
+  virtual bool needBroadphaseCollision(btBroadphaseProxy *_proxy0,
+      btBroadphaseProxy *_proxy1) const
+    {
+      GZ_ASSERT(_proxy0 != NULL && _proxy1 != NULL,
+          "Bullet broadphase overlapping pair proxies are NULL");
+
+      bool collide = (_proxy0->m_collisionFilterGroup
+          & _proxy1->m_collisionFilterMask) != 0;
+      collide = collide && (_proxy1->m_collisionFilterGroup
+          & _proxy0->m_collisionFilterMask);
+
+      btRigidBody *rb0 = btRigidBody::upcast(
+              static_cast<btCollisionObject*>(_proxy0->m_clientObject));
+      if (!rb0)
+        return collide;
+
+      btRigidBody *rb1 = btRigidBody::upcast(
+              static_cast<btCollisionObject*>(_proxy1->m_clientObject));
+      if (!rb1)
+         return collide;
+
+      BulletLink *link0 = static_cast<BulletLink *>(
+          rb0->getUserPointer());
+      GZ_ASSERT(link0 != NULL, "Link0 in collision pair is NULL");
+
+      BulletLink *link1 = static_cast<BulletLink *>(
+          rb1->getUserPointer());
+      GZ_ASSERT(link1 != NULL, "Link1 in collision pair is NULL");
+
+      if (!link0->GetSelfCollide() || !link1->GetSelfCollide())
+      {
+        if (link0->GetModel() == link1->GetModel())
+          collide = false;
+      }
+      return collide;
+    }
+};
+
+//////////////////////////////////////////////////
 bool ContactCallback(btManifoldPoint &/*_cp*/,
     const btCollisionObjectWrapper * /*_obj0*/, int /*_partId0*/,
     int /*_index0*/, const btCollisionObjectWrapper * /*_obj1*/,
   this->dynamicsWorld = new btDiscreteDynamicsWorld(this->dispatcher,
       this->broadPhase, this->solver, this->collisionConfig);
 
+  btOverlapFilterCallback *filterCallback = new CollisionFilter();
+  btOverlappingPairCache* pairCache = this->dynamicsWorld->getPairCache();
+  GZ_ASSERT(pairCache != NULL,
+      "Bullet broadphase overlapping pair cache is NULL");
+  pairCache->setOverlapFilterCallback(filterCallback);
+
   // TODO: Enable this to do custom contact setting
   gContactAddedCallback = ContactCallback;
   gContactProcessedCallback = ContactProcessed;

gazebo/physics/bullet/BulletPlaneShape.hh

                 math::Vector3 n = this->GetNormal();
                 btVector3 vec(n.x, n.y, n.z);
 
-                bParent->SetCollisionShape(new btStaticPlaneShape(vec, 0.0));
+                bParent->SetCollisionShape(new btStaticPlaneShape(vec, 0.0),
+                    false);
               }
     };
     /// \}

gazebo/physics/bullet/BulletRayShape.cc

   if (this->collisionParent)
   {
     BulletCollisionPtr collision =
-      boost::shared_static_cast<BulletCollision>(this->collisionParent);
+        boost::shared_static_cast<BulletCollision>(this->collisionParent);
 
     LinkPtr link = this->collisionParent->GetLink();
     GZ_ASSERT(link != NULL, "Bullet link is NULL");
       this->globalEndPos.z);
 
   btCollisionWorld::ClosestRayResultCallback rayCallback(start, end);
+  rayCallback.m_collisionFilterGroup = GZ_SENSOR_COLLIDE;
+  rayCallback.m_collisionFilterMask = ~GZ_SENSOR_COLLIDE;
 
   boost::recursive_mutex::scoped_lock lock(
       *this->physicsEngine->GetPhysicsUpdateMutex());
         this->globalEndPos.z);
 
     btCollisionWorld::ClosestRayResultCallback rayCallback(start, end);
+    rayCallback.m_collisionFilterGroup = GZ_SENSOR_COLLIDE;
+    rayCallback.m_collisionFilterMask = ~GZ_SENSOR_COLLIDE;
     this->physicsEngine->GetDynamicsWorld()->rayTest(
         start, end, rayCallback);
     if (rayCallback.hasHit())

gazebo/physics/bullet/BulletScrewJoint.cc

  * Date: 13 Oct 2009
  */
 
-#include "common/Console.hh"
-#include "common/Exception.hh"
+#include "gazebo/common/Assert.hh"
+#include "gazebo/common/Console.hh"
+#include "gazebo/common/Exception.hh"
 
-#include "physics/bullet/BulletLink.hh"
-#include "physics/bullet/BulletPhysics.hh"
-#include "physics/bullet/BulletTypes.hh"
-#include "physics/bullet/BulletScrewJoint.hh"
+#include "gazebo/physics/bullet/BulletLink.hh"
+#include "gazebo/physics/bullet/BulletPhysics.hh"
+#include "gazebo/physics/bullet/BulletTypes.hh"
+#include "gazebo/physics/bullet/BulletScrewJoint.hh"
 
 using namespace gazebo;
 using namespace physics;
 BulletScrewJoint::BulletScrewJoint(btDynamicsWorld *_world, BasePtr _parent)
     : ScrewJoint<BulletJoint>(_parent)
 {
-  this->world = _world;
+  GZ_ASSERT(_world, "bullet world pointer is NULL");
+  this->bulletWorld = _world;
   this->bulletScrew = NULL;
 }
 
 //////////////////////////////////////////////////
 void BulletScrewJoint::Attach(LinkPtr _one, LinkPtr _two)
 {
+  gzwarn << "Screw joint constraints are currently not enforced" << "\n";
+
   ScrewJoint<BulletJoint>::Attach(_one, _two);
 
   BulletLinkPtr bulletChildLink =
   BulletLinkPtr bulletParentLink =
     boost::shared_static_cast<BulletLink>(this->parentLink);
 
-  if (!bulletChildLink || !bulletParentLink)
-    gzthrow("Requires bullet bodies");
 
   btTransform frame1, frame2;
   frame1 = btTransform::getIdentity();
   frame2 = btTransform::getIdentity();
 
   math::Vector3 pivotA, pivotB;
+  math::Pose pose;
 
-  pivotA = this->anchorPos + this->childLink->GetWorldPose().pos
-                           - this->parentLink->GetWorldPose().pos;
+  pivotA = this->anchorPos;
   pivotB = this->anchorPos;
-
-  pivotA = this->parentLink->GetWorldPose().rot.RotateVectorReverse(pivotA);
-  pivotB = this->childLink->GetWorldPose().rot.RotateVectorReverse(pivotB);
+  // Check if parentLink exists. If not, the parent will be the world.
+  if (this->parentLink)
+  {
+    // Compute relative pose between joint anchor and CoG of parent link.
+    pose = this->parentLink->GetWorldCoGPose();
+    // Subtract CoG position from anchor position, both in world frame.
+    pivotA -= pose.pos;
+    // Rotate pivot offset and axis into body-fixed frame of parent.
+    pivotA = pose.rot.RotateVectorReverse(pivotA);
+  }
+  // Check if childLink exists. If not, the child will be the world.
+  if (this->childLink)
+  {
+    // Compute relative pose between joint anchor and CoG of child link.
+    pose = this->childLink->GetWorldCoGPose();
+    // Subtract CoG position from anchor position, both in world frame.
+    pivotB -= pose.pos;
+    // Rotate pivot offset and axis into body-fixed frame of child.
+    pivotB = pose.rot.RotateVectorReverse(pivotB);
+  }
 
   frame1.setOrigin(btVector3(pivotA.x, pivotA.y, pivotA.z));
   frame2.setOrigin(btVector3(pivotB.x, pivotB.y, pivotB.z));
   frame1.getBasis().setEulerZYX(0, M_PI*0.5, 0);
   frame2.getBasis().setEulerZYX(0, M_PI*0.5, 0);
 
-  this->bulletScrew = new btSliderConstraint(
-      *bulletChildLink->GetBulletLink(),
-      *bulletParentLink->GetBulletLink(),
-      frame2, frame1, true);
+  // If both links exist, then create a joint between the two links.
+  if (bulletChildLink && bulletParentLink)
+  {
+    this->bulletScrew = new btSliderConstraint(
+        *bulletParentLink->GetBulletLink(),
+        *bulletChildLink->GetBulletLink(),
+        frame1, frame2, true);
+  }
+  // If only the child exists, then create a joint between the child
+  // and the world.
+  else if (bulletChildLink)
+  {
+    this->bulletScrew = new btSliderConstraint(
+        *bulletChildLink->GetBulletLink(), frame2, true);
+  }
+  // If only the parent exists, then create a joint between the parent
+  // and the world.
+  else if (bulletParentLink)
+  {
+    this->bulletScrew = new btSliderConstraint(
+        *bulletParentLink->GetBulletLink(), frame1, true);
+  }
+  // Throw an error if no links are given.
+  else
+  {
+    gzthrow("joint without links\n");
+  }
 
   this->constraint = this->bulletScrew;
 
   // Add the joint to the world
-  this->world->addConstraint(this->constraint);
+  GZ_ASSERT(this->bulletWorld, "bullet world pointer is NULL");
+  this->bulletWorld->addConstraint(this->constraint);
 
   // Allows access to impulse
   this->constraint->enableFeedback(true);
 }
 
 //////////////////////////////////////////////////
-math::Angle BulletScrewJoint::GetAngle(int /*_index*/) const
+double BulletScrewJoint::GetVelocity(int /*_index*/) const
 {
-  return this->bulletScrew->getLinearPos();
+  double result = 0;
+  if (this->bulletScrew)
+    result = this->bulletScrew->getTargetLinMotorVelocity();
+  return result;
 }
 
 //////////////////////////////////////////////////
-double BulletScrewJoint::GetVelocity(int /*_index*/) const
+void BulletScrewJoint::SetVelocity(int /*_index*/, double _angle)
 {
-  gzerr << "Not implemented in bullet\n";
-  return 0;
-}
-
-//////////////////////////////////////////////////
-void BulletScrewJoint::SetVelocity(int /*_index*/, double /*_angle*/)
-{
-  gzerr << "Not implemented in bullet\n";
+  if (this->bulletScrew)
+    this->bulletScrew->setTargetLinMotorVelocity(_angle);
 }
 
 //////////////////////////////////////////////////
 }
 
 //////////////////////////////////////////////////
-void BulletScrewJoint::SetDamping(int /*index*/, double /*_damping*/)
+void BulletScrewJoint::SetDamping(int /*index*/, double _damping)
 {
-  gzerr << "Not implemented\n";
+  if (this->bulletScrew)
+    this->bulletScrew->setDampingDirLin(_damping);
 }
 
 //////////////////////////////////////////////////
 //////////////////////////////////////////////////
 void BulletScrewJoint::SetHighStop(int /*_index*/, const math::Angle &_angle)
 {
-  this->bulletScrew->setUpperLinLimit(_angle.Radian());
+  if (this->bulletScrew)
+    this->bulletScrew->setUpperLinLimit(_angle.Radian());
 }
 
 //////////////////////////////////////////////////
 void BulletScrewJoint::SetLowStop(int /*_index*/, const math::Angle &_angle)
 {
-  this->bulletScrew->setLowerLinLimit(_angle.Radian());
+  if (this->bulletScrew)
+    this->bulletScrew->setLowerLinLimit(_angle.Radian());
 }
 
 //////////////////////////////////////////////////
 math::Angle BulletScrewJoint::GetHighStop(int /*_index*/)
 {
-  return this->bulletScrew->getUpperLinLimit();
+  math::Angle result;
+  if (this->bulletScrew)
+    result = this->bulletScrew->getUpperLinLimit();
+  return result;
 }
 
 //////////////////////////////////////////////////
 math::Angle BulletScrewJoint::GetLowStop(int /*_index*/)
 {
-  return this->bulletScrew->getLowerLinLimit();
+  math::Angle result;
+  if (this->bulletScrew)
+    result = this->bulletScrew->getLowerLinLimit();
+  return result;
 }
 
 //////////////////////////////////////////////////
-void BulletScrewJoint::SetMaxForce(int /*_index*/, double /*_force*/)
+void BulletScrewJoint::SetMaxForce(int /*_index*/, double _force)
 {
-  gzerr << "Not implemented\n";
+  if (this->bulletScrew)
+    this->bulletScrew->setMaxLinMotorForce(_force);
 }
 
 //////////////////////////////////////////////////
 double BulletScrewJoint::GetMaxForce(int /*index*/)
 {
-  gzerr << "Not implemented\n";
-  return 0;
+  double result = 0;
+  if (this->bulletScrew)
+    result = this->bulletScrew->getMaxLinMotorForce();
+  return result;
 }
 
 //////////////////////////////////////////////////
 math::Vector3 BulletScrewJoint::GetGlobalAxis(int /*_index*/) const
 {
-  gzerr << "BulletScrewJoint::GetGlobalAxis not implemented\n";
-  return math::Vector3();
+  math::Vector3 result;
+  if (this->bulletScrew)
+  {
+    // I have not verified the following math, though I based it on internal
+    // bullet code at line 250 of btHingeConstraint.cpp
+    btVector3 vec =
+      this->bulletScrew->getRigidBodyA().getCenterOfMassTransform().getBasis() *
+      this->bulletScrew->getFrameOffsetA().getBasis().getColumn(2);
+    result = BulletTypes::ConvertVector3(vec);
+  }
+  else
+    gzwarn << "bulletHinge does not exist, returning fake axis\n";
+  return result;
 }
 
 //////////////////////////////////////////////////
 math::Angle BulletScrewJoint::GetAngleImpl(int /*_index*/) const
 {
-  gzerr << "BulletScrewJoint::GetAngleImpl not implemented\n";
-  return math::Angle();
+  math::Angle result;
+  if (this->bulletScrew)
+    result = this->bulletScrew->getLinearPos();
+  return result;
 }

gazebo/physics/bullet/BulletScrewJoint.hh

       /// \brief Get the low stop of an axis(index).
       public: virtual math::Angle GetLowStop(int _index);
 
-      /// \brief Get the position of the joint
-      public: virtual math::Angle GetAngle(int _index) const;
-
       /// \brief Get the rate of change
       public: virtual double GetVelocity(int _index) const;
 

gazebo/physics/bullet/BulletSliderJoint.cc

  * Date: 13 Oct 2009
  */
 
-#include "common/Console.hh"
-#include "common/Exception.hh"
+#include "gazebo/common/Assert.hh"
+#include "gazebo/common/Console.hh"
+#include "gazebo/common/Exception.hh"
 
-#include "physics/bullet/bullet_inc.h"
-#include "physics/bullet/BulletLink.hh"
-#include "physics/bullet/BulletPhysics.hh"
-#include "physics/bullet/BulletSliderJoint.hh"
+#include "gazebo/physics/bullet/bullet_inc.h"
+#include "gazebo/physics/bullet/BulletLink.hh"
+#include "gazebo/physics/bullet/BulletPhysics.hh"
+#include "gazebo/physics/bullet/BulletSliderJoint.hh"
 
 using namespace gazebo;
 using namespace physics;
 BulletSliderJoint::BulletSliderJoint(btDynamicsWorld *_world, BasePtr _parent)
     : SliderJoint<BulletJoint>(_parent)
 {
-  this->world = _world;
+  GZ_ASSERT(_world, "bullet world pointer is NULL");
+  this->bulletWorld = _world;
   this->bulletSlider = NULL;
 }
 
 }
 
 //////////////////////////////////////////////////
-void BulletSliderJoint::Attach(LinkPtr _one, LinkPtr _two)
+void BulletSliderJoint::Init()
 {
-  SliderJoint<BulletJoint>::Attach(_one, _two);
+  SliderJoint<BulletJoint>::Init();
 
   BulletLinkPtr bulletChildLink =
     boost::shared_static_cast<BulletLink>(this->childLink);
   BulletLinkPtr bulletParentLink =
     boost::shared_static_cast<BulletLink>(this->parentLink);
 
-  if (!bulletChildLink || !bulletParentLink)
-    gzthrow("Requires bullet bodies");
+  // Get axis unit vector (expressed in world frame).
+  math::Vector3 axis = this->initialWorldAxis;
+  if (axis == math::Vector3::Zero)
+  {
+    gzerr << "axis must have non-zero length, resetting to 0 0 1\n";
+    axis.Set(0, 0, 1);
+  }
 
-  btVector3 anchor, axis1, axis2;
-  btTransform frame1, frame2;
-  frame1 = btTransform::getIdentity();
-  frame2 = btTransform::getIdentity();
+  // Local variables used to compute pivots and axes in body-fixed frames
+  // for the parent and child links.
+  math::Vector3 pivotParent, pivotChild, axisParent, axisChild;
+  math::Pose pose;
+  btTransform frameParent, frameChild;
+  btVector3 axis2, axis3;
 
-  math::Vector3 pivotA, pivotB;
+  // Initialize pivots to anchorPos, which is expressed in the
+  // world coordinate frame.
+  pivotParent = this->anchorPos;
+  pivotChild = this->anchorPos;
 
-  pivotA = this->anchorPos + this->childLink->GetWorldPose().pos
-                           - this->parentLink->GetWorldPose().pos;
-  pivotB = this->anchorPos;
+  // Check if parentLink exists. If not, the parent will be the world.
+  if (this->parentLink)
+  {
+    // Compute relative pose between joint anchor and CoG of parent link.
+    pose = this->parentLink->GetWorldCoGPose();
+    // Subtract CoG position from anchor position, both in world frame.
+    pivotParent -= pose.pos;
+    // Rotate pivot offset and axis into body-fixed frame of parent.
+    pivotParent = pose.rot.RotateVectorReverse(pivotParent);
+    frameParent.setOrigin(BulletTypes::ConvertVector3(pivotParent));
+    axisParent = pose.rot.RotateVectorReverse(axis);
+    axisParent = axisParent.Normalize();
+    // The following math is based on btHingeConstraint.cpp:95-115
+    btPlaneSpace1(BulletTypes::ConvertVector3(axisParent), axis2, axis3);
+    frameParent.getBasis().setValue(
+      axisParent.x, axis2.x(), axis3.x(),
+      axisParent.y, axis2.y(), axis3.y(),
+      axisParent.z, axis2.z(), axis3.z());
+  }
+  // Check if childLink exists. If not, the child will be the world.
+  if (this->childLink)
+  {
+    // Compute relative pose between joint anchor and CoG of child link.
+    pose = this->childLink->GetWorldCoGPose();
+    // Subtract CoG position from anchor position, both in world frame.
+    pivotChild -= pose.pos;
+    // Rotate pivot offset and axis into body-fixed frame of child.
+    pivotChild = pose.rot.RotateVectorReverse(pivotChild);
+    frameChild.setOrigin(BulletTypes::ConvertVector3(pivotChild));
+    axisChild = pose.rot.RotateVectorReverse(axis);
+    axisChild = axisChild.Normalize();
+    // The following math is based on btHingeConstraint.cpp:95-115
+    btPlaneSpace1(BulletTypes::ConvertVector3(axisChild), axis2, axis3);
+    frameChild.getBasis().setValue(
+      axisChild.x, axis2.x(), axis3.x(),
+      axisChild.y, axis2.y(), axis3.y(),
+      axisChild.z, axis2.z(), axis3.z());
+  }
 
-  pivotA = this->parentLink->GetWorldPose().rot.RotateVectorReverse(pivotA);
-  pivotB = this->childLink->GetWorldPose().rot.RotateVectorReverse(pivotB);
+  // If both links exist, then create a joint between the two links.
+  if (bulletChildLink && bulletParentLink)
+  {
+    this->bulletSlider = new btSliderConstraint(
+        *bulletParentLink->GetBulletLink(),
+        *bulletChildLink->GetBulletLink(),
+        frameParent, frameChild, true);
+  }
+  // If only the child exists, then create a joint between the child
+  // and the world.
+  else if (bulletChildLink)
+  {
+    this->bulletSlider = new btSliderConstraint(
+        *bulletChildLink->GetBulletLink(), frameChild, true);
+  }
+  // If only the parent exists, then create a joint between the parent
+  // and the world.
+  else if (bulletParentLink)
+  {
+    this->bulletSlider = new btSliderConstraint(
+        *bulletParentLink->GetBulletLink(), frameParent, true);
+  }
+  // Throw an error if no links are given.
+  else
+  {
+    gzthrow("joint without links\n");
+  }
 
-  std::cout << "AnchorPos[" << this->anchorPos << "]\n";
-  std::cout << "Slider PivotA[" << pivotA << "] PivotB[" << pivotB << "]\n";
+  if (!this->bulletSlider)
+    gzthrow("unable to create bullet slider joint\n");
 
-  frame1.setOrigin(btVector3(pivotA.x, pivotA.y, pivotA.z));
-  frame2.setOrigin(btVector3(pivotB.x, pivotB.y, pivotB.z));
+  // btSliderConstraint has 2 degrees-of-freedom (like a piston)
+  // so disable the rotation.
+  this->bulletSlider->setLowerAngLimit(0.0);
+  this->bulletSlider->setUpperAngLimit(0.0);
 
-  frame1.getBasis().setEulerZYX(0, M_PI*0.5, 0);
-  frame2.getBasis().setEulerZYX(0, M_PI*0.5, 0);
-
-  this->bulletSlider = new btSliderConstraint(
-      *bulletChildLink->GetBulletLink(),
-      *bulletParentLink->GetBulletLink(),
-      frame2, frame1, true);
-
-  // this->bulletSlider->setLowerAngLimit(0.0);
-  // this->bulletSlider->setUpperAngLimit(0.0);
-
-  double pos = this->bulletSlider->getLinearPos();
-  this->bulletSlider->setLowerLinLimit(pos);
-  this->bulletSlider->setUpperLinLimit(pos+0.9);
+  // Apply joint translation limits here.
+  // TODO: velocity and effort limits.
+  GZ_ASSERT(this->sdf != NULL, "Joint sdf member is NULL");
+  sdf::ElementPtr limitElem;
+  limitElem = this->sdf->GetElement("axis")->GetElement("limit");
+  this->bulletSlider->setLowerLinLimit(limitElem->GetValueDouble("lower"));
+  this->bulletSlider->setUpperLinLimit(limitElem->GetValueDouble("upper"));
 
   this->constraint = this->bulletSlider;
 
   // Add the joint to the world
-  this->world->addConstraint(this->bulletSlider, true);
+  GZ_ASSERT(this->bulletWorld, "bullet world pointer is NULL");
+  this->bulletWorld->addConstraint(this->bulletSlider, true);
 
   // Allows access to impulse
   this->constraint->enableFeedback(true);
 }
 
 //////////////////////////////////////////////////
-math::Angle BulletSliderJoint::GetAngle(int /*_index*/) const
-{
-  return static_cast<btSliderConstraint*>(this->constraint)->getLinearPos();
-}
-
-//////////////////////////////////////////////////
 double BulletSliderJoint::GetVelocity(int /*_index*/) const
 {
-  gzerr << "Not implemented in bullet\n";
-  return 0;
+  double result = 0;
+  // I'm not sure this will work
+  if (this->bulletSlider)
+    result = this->bulletSlider->getTargetLinMotorVelocity();
+  return result;
 }
 
 //////////////////////////////////////////////////
 void BulletSliderJoint::SetVelocity(int /*_index*/, double _angle)
 {
-  this->bulletSlider->setTargetLinMotorVelocity(_angle);
+  if (this->bulletSlider)
+    this->bulletSlider->setTargetLinMotorVelocity(_angle);
 }
 
 //////////////////////////////////////////////////
-void BulletSliderJoint::SetAxis(int /*_index*/, const math::Vector3 &/*_axis*/)
+void BulletSliderJoint::SetAxis(int /*_index*/, const math::Vector3 &_axis)
 {
-  gzerr << "Not implemented in bullet\n";
+  // Note that _axis is given in a world frame,
+  // but bullet uses a body-fixed frame
+  if (!this->bulletSlider)
+  {
+    // this hasn't been initialized yet, store axis in initialWorldAxis
+    this->initialWorldAxis = _axis;
+  }
+  else
+  {
+    gzerr << "SetAxis for existing joint is not implemented\n";
+  }
 }
 
 //////////////////////////////////////////////////
 void BulletSliderJoint::SetDamping(int /*index*/, const double _damping)
 {
-  this->bulletSlider->setDampingDirLin(_damping);
+  if (this->bulletSlider)
+    this->bulletSlider->setDampingDirLin(_damping);
 }
 
 //////////////////////////////////////////////////
     _effort = math::clamp(_effort, -this->effortLimit[_index],
        this->effortLimit[_index]);
 
-  /*btVector3 hingeAxisLocal = this->bulletSlider->getAFrame().getBasis().getColumn(2); // z-axis of constraint frame
-  btVector3 hingeAxisWorld = this->bulletSlider->getRigidBodyA().getWorldTransform().getBasis() * hingeAxisLocal;
+  if (this->bulletSlider && this->constraint)
+  {
+    // x-axis of constraint frame
+    btVector3 hingeAxisLocalA =
+      this->bulletSlider->getFrameOffsetA().getBasis().getColumn(0);
+    btVector3 hingeAxisLocalB =
+      this->bulletSlider->getFrameOffsetB().getBasis().getColumn(0);
 
-  btVector3 hingeTorque = _torque * hingeAxisWorld;
-  */
+    btVector3 hingeAxisWorldA =
+      this->bulletSlider->getRigidBodyA().getWorldTransform().getBasis() *
+      hingeAxisLocalA;
+    btVector3 hingeAxisWorldB =
+      this->bulletSlider->getRigidBodyB().getWorldTransform().getBasis() *
+      hingeAxisLocalB;
 
-  btVector3 force(0, 0, _effort);
-  this->constraint->getRigidBodyA().applyCentralForce(force);
-  this->constraint->getRigidBodyB().applyCentralForce(-force);
+    btVector3 hingeForceA = _effort * hingeAxisWorldA;
+    btVector3 hingeForceB = _effort * hingeAxisWorldB;
+
+    // TODO: switch to applyForce and specify body-fixed offset
+    this->constraint->getRigidBodyA().applyCentralForce(-hingeForceA);
+    this->constraint->getRigidBodyB().applyCentralForce(hingeForceB);
+  }
 }
 
 //////////////////////////////////////////////////
 void BulletSliderJoint::SetHighStop(int /*_index*/,
-                                    const math::Angle &/*_angle*/)
+                                    const math::Angle &_angle)
 {
-  // this->bulletSlider->setUpperLinLimit(_angle.Radian());
+  Joint::SetHighStop(0, _angle);
+  if (this->bulletSlider)
+    this->bulletSlider->setUpperLinLimit(_angle.Radian());
 }
 
 //////////////////////////////////////////////////
 void BulletSliderJoint::SetLowStop(int /*_index*/,
-                                   const math::Angle &/*_angle*/)
+                                   const math::Angle &_angle)
 {
-  // this->bulletSlider->setLowerLinLimit(_angle.Radian());
+  Joint::SetLowStop(0, _angle);
+  if (this->bulletSlider)
+    this->bulletSlider->setLowerLinLimit(_angle.Radian());
 }
 
 //////////////////////////////////////////////////
 math::Angle BulletSliderJoint::GetHighStop(int /*_index*/)
 {
-  return this->bulletSlider->getUpperLinLimit();
+  math::Angle result;
+  if (this->bulletSlider)
+    result = this->bulletSlider->getUpperLinLimit();
+  else
+    gzerr << "Joint must be created before getting high stop\n";
+  return result;
 }
 
 //////////////////////////////////////////////////
 math::Angle BulletSliderJoint::GetLowStop(int /*_index*/)
 {
-  return this->bulletSlider->getLowerLinLimit();
+  math::Angle result;
+  if (this->bulletSlider)
+    result = this->bulletSlider->getLowerLinLimit();
+  else
+    gzerr << "Joint must be created before getting low stop\n";
+  return result;
 }
 
 //////////////////////////////////////////////////
 void BulletSliderJoint::SetMaxForce(int /*_index*/, double _force)
 {
-  this->bulletSlider->setMaxLinMotorForce(_force);
+  if (this->bulletSlider)
+    this->bulletSlider->setMaxLinMotorForce(_force);
 }
 
 //////////////////////////////////////////////////
 double BulletSliderJoint::GetMaxForce(int /*_index*/)
 {
-  return this->bulletSlider->getMaxLinMotorForce();
+  double result = 0;
+  if (this->bulletSlider)
+    result = this->bulletSlider->getMaxLinMotorForce();
+  return result;
 }
 
 //////////////////////////////////////////////////
 math::Vector3 BulletSliderJoint::GetGlobalAxis(int /*_index*/) const
 {
-  gzerr << "Not implemented\n";
-  return math::Vector3();
+  math::Vector3 result;
+  if (this->bulletSlider)
+  {
+    // I have not verified the following math, though I based it on internal
+    // bullet code at line 250 of btHingeConstraint.cpp
+    btVector3 vec =
+      this->bulletSlider->getRigidBodyA().getCenterOfMassTransform().getBasis()
+      * this->bulletSlider->getFrameOffsetA().getBasis().getColumn(2);
+    result = BulletTypes::ConvertVector3(vec);
+  }
+  else
+    gzwarn << "bulletHinge does not exist, returning fake axis\n";
+  return result;
 }
 
 //////////////////////////////////////////////////
 math::Angle BulletSliderJoint::GetAngleImpl(int /*_index*/) const
 {
-  gzerr << "Not implemented\n";
-  return math::Angle();
+  math::Angle result;
+  if (this->bulletSlider)
+    result = this->bulletSlider->getLinearPos();
+  else
+    gzwarn << "bulletSlider does not exist, returning default position\n";
+  return result;
 }
-
-

gazebo/physics/bullet/BulletSliderJoint.hh

       /// \brief Load the BulletSliderJoint
       protected: virtual void Load(sdf::ElementPtr _sdf);
 
-      /// \brief Attach the two bodies with this joint
-      public: void Attach(LinkPtr _one, LinkPtr _two);
+      // Documentation inherited.
+      protected: virtual void Init();
 
-      /// \brief Set the axis of motion
-      public: void SetAxis(int _index, const math::Vector3 &_axis);
+      // Documentation inherited.
+      public: virtual void SetAxis(int _index, const math::Vector3 &_axis);
 
       /// \brief Set joint damping, not yet implemented
       public: virtual void SetDamping(int _index, const double _damping);
       /// \brief Get the low stop of an axis(index).
       public: virtual math::Angle GetLowStop(int _index);
 
-      /// \brief Get the position of the joint
-      public: virtual math::Angle GetAngle(int _index) const;
-
       /// \brief Get the rate of change
       public: virtual double GetVelocity(int _index) const;
 
 
       /// \brief Pointer to bullet slider constraint
       private: btSliderConstraint *bulletSlider;
+
+      /// \brief Initial value of joint axis, expressed as unit vector
+      ///        in world frame.
+      private: math::Vector3 initialWorldAxis;
     };
 
   /// \}
   }
 }
 #endif
-

gazebo/physics/bullet/BulletUniversalJoint.cc

  * Date: 24 May 2009
  */
 
-#include "common/Exception.hh"
-#include "common/Console.hh"
-#include "physics/bullet/BulletLink.hh"
-#include "physics/bullet/BulletTypes.hh"
-#include "physics/bullet/BulletUniversalJoint.hh"
+#include "gazebo/common/Assert.hh"
+#include "gazebo/common/Console.hh"
+#include "gazebo/common/Exception.hh"
+
+#include "gazebo/physics/bullet/BulletLink.hh"
+#include "gazebo/physics/bullet/BulletTypes.hh"
+#include "gazebo/physics/bullet/BulletUniversalJoint.hh"
 
 using namespace gazebo;
 using namespace physics;
 BulletUniversalJoint::BulletUniversalJoint(btDynamicsWorld *_world,
   BasePtr _parent) : UniversalJoint<BulletJoint>(_parent)
 {
-  this->world = _world;
+  GZ_ASSERT(_world, "bullet world pointer is NULL");
+  this->bulletWorld = _world;
   this->bulletUniversal = NULL;
 }
 
   this->constraint = this->bulletUniversal;
 
   // Add the joint to the world
-  this->world->addConstraint(this->bulletUniversal, true);
+  GZ_ASSERT(this->bulletWorld, "bullet world pointer is NULL");
+  this->bulletWorld->addConstraint(this->bulletUniversal, true);
 
   // Allows access to impulse
   this->bulletUniversal->enableFeedback(true);

gazebo/physics/ode/ODECollision.hh

 
       /// \brief Set the encapsulated collsion object.
       /// \param[in] _collisionId ODE id of the collision object.
-      /// \param[in] _placeable True to make the object m.
+      /// \param[in] _placeable True to make the object movable.
       public: void SetCollision(dGeomID _collisionId, bool _placeable);
 
       /// \brief Return the collision id.

gazebo/physics/ode/ODEJoint.cc

 //////////////////////////////////////////////////
 void ODEJoint::SetHighStop(int _index, const math::Angle &_angle)
 {
+  Joint::SetHighStop(_index, _angle);
   switch (_index)
   {
     case 0:
 //////////////////////////////////////////////////
 void ODEJoint::SetLowStop(int _index, const math::Angle &_angle)
 {
+  Joint::SetLowStop(_index, _angle);
   switch (_index)
   {
     case 0:

gazebo/transport/Transport.cc

 std::list<msgs::Request *> g_requests;
 std::list<boost::shared_ptr<msgs::Response> > g_responses;
 
+#define DEFAULT_MASTER_PORT 11345
+
 /////////////////////////////////////////////////
 bool transport::get_master_uri(std::string &_masterHost,
                                unsigned int &_masterPort)
 {
-  char *char_uri = getenv("GAZEBO_MASTER_URI");
+  char *charURI = getenv("GAZEBO_MASTER_URI");
 
   // Set to default host and port
-  if (!char_uri || strlen(char_uri) == 0)
+  if (!charURI || strlen(charURI) == 0)
   {
     _masterHost = "localhost";
-    _masterPort = 11345;
+    _masterPort = DEFAULT_MASTER_PORT;
     return false;
   }
 
-  std::string master_uri = char_uri;
+  std::string masterURI = charURI;
 
-  boost::replace_first(master_uri, "http://", "");
-  int last_colon = master_uri.find_last_of(":");
-  _masterHost = master_uri.substr(0, last_colon);
-  _masterPort = boost::lexical_cast<unsigned int>(
-      master_uri.substr(last_colon+1, master_uri.size() - (last_colon+1)));
+  boost::replace_first(masterURI, "http://", "");
+  size_t lastColon = masterURI.find_last_of(":");
+  _masterHost = masterURI.substr(0, lastColon);
+
+  if (lastColon == std::string::npos)
+  {
+    gzerr << "Port missing in master URI[" << masterURI
+          << "]. Using default value of " << DEFAULT_MASTER_PORT << ".\n";
+    _masterPort = DEFAULT_MASTER_PORT;
+  }
+  else
+  {
+    _masterPort = boost::lexical_cast<unsigned int>(
+        masterURI.substr(lastColon + 1, masterURI.size() - (lastColon + 1)));
+  }
 
   return true;
 }

sdf/worlds/revolute_joint_test.world

+<sdf version="1.3">
+  <world name="default">
+
+    <include><uri>model://sun</uri></include>
+    <include><uri>model://ground_plane</uri></include>
+  
+    <include>
+      <uri>model://double_pendulum_with_base</uri>
+      <name>pendulum_0deg</name>
+      <pose>0 2.1 0  0 0 0</pose>
+    </include>
+
+    <include>
+      <uri>model://double_pendulum_with_base</uri>
+      <name>pendulum_180deg</name>
+      <pose>0 -2.1 0  0 0 3.1416</pose>
+    </include>
+
+    <include>
+      <uri>model://double_pendulum_with_base</uri>
+      <name>pendulum_90deg</name>
+      <pose>-2.1 0 0  0 0 1.5708</pose>
+    </include>
+
+    <include>
+      <uri>model://double_pendulum_with_base</uri>
+      <name>pendulum_270deg</name>
+      <pose>2.1 0 0  0 0 -1.5708</pose>
+    </include>
+
+    <include>
+      <uri>model://double_pendulum_with_base</uri>
+      <name>pendulum_315deg</name>
+      <pose>1.48 1.48 0  0 0 -0.7854</pose>
+    </include>
+
+    <include>
+      <uri>model://double_pendulum_with_base</uri>
+      <name>pendulum_225deg</name>
+      <pose>1.48 -1.48 0  0 0 -2.3562</pose>
+      <pose>0 -2.1 0  0 0 </pose>
+    </include>
+
+    <include>
+      <uri>model://double_pendulum_with_base</uri>
+      <name>pendulum_135deg</name>
+      <pose>-1.48 -1.48 0  0 0 2.3562</pose>
+    </include>
+
+    <include>
+      <uri>model://double_pendulum_with_base</uri>
+      <name>pendulum_45deg</name>
+      <pose>-1.48 1.48 0  0 0 0.7854</pose>
+    </include>
+  </world>
+</sdf>

test/ServerFixture.hh

              }
 
   protected: void SpawnCylinder(const std::string &_name,
-                 const math::Vector3 &_pos, const math::Vector3 &_rpy)
+                 const math::Vector3 &_pos, const math::Vector3 &_rpy,
+                 bool _static = false)
              {
                msgs::Factory msg;
                std::ostringstream newModelStr;
 
                newModelStr << "<sdf version='" << SDF_VERSION << "'>"
                  << "<model name ='" << _name << "'>"
+                 << "<static>" << _static << "</static>"
                  << "<pose>" << _pos.x << " "
                              << _pos.y << " "
                              << _pos.z << " "
 
   protected: void SpawnSphere(const std::string &_name,
                  const math::Vector3 &_pos, const math::Vector3 &_rpy,
-                 bool _wait = true)
+                 bool _wait = true, bool _static = false)
              {
                msgs::Factory msg;
                std::ostringstream newModelStr;
 
                newModelStr << "<sdf version='" << SDF_VERSION << "'>"
                  << "<model name ='" << _name << "'>"
+                 << "<static>" << _static << "</static>"
                  << "<pose>" << _pos.x << " "
                              << _pos.y << " "
                              << _pos.z << " "
   protected: void SpawnSphere(const std::string &_name,
                  const math::Vector3 &_pos, const math::Vector3 &_rpy,
                  const math::Vector3 &_cog, double _radius,
-                 bool _wait = true)
+                 bool _wait = true, bool _static = false)
              {