Commits

John Hsu  committed 40a91b2 Merge

merging from gazebo_3.0

  • Participants
  • Parent commits 58deb0c, c958409
  • Branches gazebo_3.0_dart_4.0

Comments (0)

Files changed (50)

File Migration.md

     + ***Deprecation*** void CFMDamping()
     + ***Replacement*** void ApplyImplicitStiffnessDamping()
 
+1. **gazebo/physics/ScrewJoint.hh**
+    + ***Deprecation*** virtual void SetThreadPitch(unsigned int _index, double _threadPitch) = 0
+    + ***Replacement*** virtual void SetThreadPitch(double _threadPitch) = 0
+    ---
+    + ***Deprecation*** virtual void GetThreadPitch(unsigned int _index) = 0
+    + ***Replacement*** virtual void GetThreadPitch() = 0
+
+1. **gazebo/physics/bullet/BulletScrewJoint.hh**
+    + ***Deprecation*** protected: virtual void Load(sdf::ElementPtr _sdf)
+    + ***Replacement*** public: virtual void Load(sdf::ElementPtr _sdf)
+
 1. **gazebo/physics/PhysicsEngine.hh**
     + ***Deprecation*** virtual void SetSORPGSPreconIters(unsigned int _iters)
     + ***Replacement*** virtual bool SetParam(const std::string &_key, const boost::any &_value)
     + virtual void SetAttribute(const std::string &_key, unsigned int _index, const boost::any &_value)
     + virtual double GetAttribute(const std::string &_key, unsigned int _index)
 
+1. **gazebo/physics/simbody/SimbodyScrewJoint.hh**
+    + virtual void SetThreadPitch(double _threadPitch)
+    + virtual void GetThreadPitch() 
+
+1. **gazebo/physics/ode/ODEScrewJoint.hh**
+    + virtual void SetThreadPitch(double _threadPitch)
+    + virtual void GetThreadPitch() 
+
+1. **gazebo/physics/ScrewJoint.hh**
+    + virtual math::Vector3 GetAnchor(unsigned int _index) const
+    + virtual void SetAnchor(unsigned int _index, const math::Vector3 &_anchor)
+
+1. **gazebo/physics/bullet/BulletJoint.hh**
+    + virtual math::Angle GetHighStop(unsigned int _index)
+    + virtual math::Angle GetLowStop(unsigned int _index)
+
 1. **gazebo/physics/simbody/SimbodyPhysics.hh**
     + virtual boost::any GetParam(const std::string &_key) const
     + virtual bool SetParam(const std::string &_key, const boost::any &_value)
     + virtual boost::any GetParam(const std::string &_key) const
     + virtual bool SetParam(const std::string &_key, const boost::any &_value)
 
-1. **gazebo/physics/Link.hh**
-    + common::MovingWindowFilter<math::Vector3> *GetLinVelFil() const
-    + common::MovingWindowFilter<math::Vector3> *GetAngVelFil() const
-
 ### Deletions
 
 1. **Removed libtool**

File deps/opende/include/ode/objects.h

File contents unchanged.

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

     dSetZero( qrel, 4 );
     limot.init( world );
     cumulative_angle = 0;
-    thread_pitch = 0.0;  // rad/m (3141.6 rad/m is about 0.002 m/rev)
+    thread_pitch = 1.0;  // rad/m (3141.6 rad/m is about 0.002 m/rev)
 }
 
 
                                      axis1, qrel );
         // from angle, update cumulative_angle, which does not wrap
         cumulative_angle = cumulative_angle +
-          shortest_angular_distance(cumulative_angle,angle);
+          shortest_angular_distance(cumulative_angle, angle);
 
         // printf("angle: %f lo[%f] hi[%f]\n", cumulative_angle,
         //   limot.lostop, limot.histop);
       {
           pos2 = node[1].body->posr.pos;
           R2 = node[1].body->posr.R;
-          for (int i = 0; i < 3; i++ )
+          for (int i = 0; i < 3; ++i )
           {
               // store distance between cg's in cgdiff
               cgdiff[i] = pos2[i] - pos1[i];
             q[2] = node[0].body->posr.pos[2] - offset[2];
         }
         lin_disp = dCalcVectorDot3 ( ax1, q );
-        lin_err = -(lin_disp-cumulative_angle*thread_pitch);
+        lin_err = -(lin_disp*thread_pitch-cumulative_angle);
         // printf("lin disp: %f lin err: %f\n", lin_disp, lin_err);
       }
 
       // so that sliding along the slider axis is disregarded. for symmetry we
       // also substitute (w1+w2)/2 for w1, as w1 is supposed to equal w2.
 
-      dVector3 ax1; // joint axis in global coordinates (unit length)
-      dVector3 p, q; // plane space of ax1
+      // ax1 is axis1 converted to body1 frame
+      dVector3 ax1;
       dMultiply0_331 ( ax1, R1, axis1 );
+
+      // p and q are vectors perpendicular to ax1 in body1 frame
+      dVector3 p, q;
       dPlaneSpace ( ax1, p, q );
+
+      // linear constraints for the hinge joint
+      // perpendicular to the sliding axis direction.
+      for (int i = 0; i < 3; ++i ) info->J1l[s0+i] = p[i];
+      for (int i = 0; i < 3; ++i ) info->J1l[s1+i] = q[i];
+
+      // if p and q do not pass through body CG's,
+      // we need to add angular constraints to balance out the forces
+      // from these linear constraints.  See below:
+
+      // a1 and a2 are axis vectors in the body frame
+      // (whereas anchor1 and anchor2 are in world frame).
+      // anchor1 is the vector from CG to joint anchor in world frame.
       dVector3 a1, a2;
+      dMultiply0_331( a1, R1, anchor1 );
 
+      // tmpp is a vector perpendicular to a1 and p in body frame,
+      // it is the direction of the angular constraint that will
+      // cancel out moment generated by linear constraint p
+      // if p does not pass through CG.
+      {
+        dVector3 tmpp;
+        dCalcVectorCross3(tmpp, p, a1);
+        for (int i = 0; i < 3; ++i ) info->J1a[s0+i] = -tmpp[i];
+      }
+
+      // tmpq is similar to tmpp, but for q.
+      {
+        dVector3 tmpq;
+        dCalcVectorCross3(tmpq, q, a1);
+        for (int i = 0; i < 3; ++i ) info->J1a[s1+i] = -tmpq[i];
+      }
+
+      // screw constraint:
+      // now constrain the sliding axis by rotation of the other body
+      for (int i = 0; i < 3; ++i ) info->J1l[s2+i] = ax1[i]*thread_pitch;
+      for (int i = 0; i < 3; ++i ) info->J1a[s2+i] = -ax1[i];
+
+      // repeat above for child body if one exists
       if ( node[1].body )
       {
-        // angular constraints if axis are not aligned with cg's
+        // linear constraints for s0 and s1
+        for (int i = 0; i < 3; ++i ) info->J2l[s0+i] = -p[i];
+        for (int i = 0; i < 3; ++i ) info->J2l[s1+i] = -q[i];
+
+        // angular compensation if p and q do not pass through CG
         dMultiply0_331( a2, R2, anchor2 );
         dVector3 tmpp;
         dCalcVectorCross3(tmpp, p, a2);
-        for (int i = 0; i < 3; i++ ) info->J2a[s0+i] = tmpp[i];
+        for (int i = 0; i < 3; ++i ) info->J2a[s0+i] = tmpp[i];
         dVector3 tmpq;
         dCalcVectorCross3(tmpq, q, a2);
-        for (int i = 0; i < 3; i++ ) info->J2a[s1+i] = tmpq[i];
-
-        // linear constraints for s0 and s1
-        for (int i = 0; i < 3; i++ ) info->J2l[s0+i] = -p[i];
-        for (int i = 0; i < 3; i++ ) info->J2l[s1+i] = -q[i];
+        for (int i = 0; i < 3; ++i ) info->J2a[s1+i] = tmpq[i];
 
         // screw constraint:
         // constrain the sliding axis by rotation of the other body
-        for (int i = 0; i < 3; i++ ) info->J2a[s2+i] =  ax1[i]*thread_pitch;
-        for (int i = 0; i < 3; i++ ) info->J2l[s2+i] = -ax1[i];
+        for (int i = 0; i < 3; ++i ) info->J2a[s2+i] =  ax1[i];
+        for (int i = 0; i < 3; ++i ) info->J2l[s2+i] = -ax1[i]*thread_pitch;
       }
 
-      // angular constraints if axis are not aligned with cg's
-      dMultiply0_331( a1, R1, anchor1 );
-      dVector3 tmpp;
-      dCalcVectorCross3(tmpp, p, a1);
-      for (int i = 0; i < 3; i++ ) info->J1a[s0+i] = -tmpp[i];
-      dVector3 tmpq;
-      dCalcVectorCross3(tmpq, q, a1);
-      for (int i = 0; i < 3; i++ ) info->J1a[s1+i] = -tmpq[i];
-
       // debug
       // printf ("anchor1 %f %f %f\n", anchor1[0], anchor1[1], anchor1[2]);
       // printf ("a1 %f %f %f\n", a1[0], a1[1], a1[2]);
       // info->J1a[s1+0] = -1;
       // info->J1a[s1+1] = 0;
       // info->J1a[s1+2] = 0;
-
-      // linear part of the hinge joint
-      for (int i = 0; i < 3; i++ ) info->J1l[s0+i] = p[i];
-      for (int i = 0; i < 3; i++ ) info->J1l[s1+i] = q[i];
-
-      // screw constraint:
-      // now constrain the sliding axis by rotation of the other body
-      for (int i = 0; i < 3; i++ ) info->J1l[s2+i] = ax1[i];
-      for (int i = 0; i < 3; i++ ) info->J1a[s2+i] = -ax1[i]*thread_pitch;
       // printf("screw err lin[%f], ang[%f], diff[%f] [%d] tp[%f]\n",
       //   thread_pitch*lin_disp, cumulative_angle, lin_err,
       //   (int)this->use_damping, thread_pitch);
       {
           // dVector3 ofs;  // offset point in global coordinates
           // dMultiply0_331 ( ofs, R2, offset );
-          // for (int i = 0; i < 3; i++ ) cgdiff[i] += ofs[i];
+          // for (int i = 0; i < 3; ++i ) cgdiff[i] += ofs[i];
 
           // error between body anchors
           dVector3 error12;
     {
         // get body2 + offset point in global coordinates
         dMultiply0_331 ( q, joint->node[1].body->posr.R, joint->offset );
-        for (int i = 0; i < 3; i++ )
+        for (int i = 0; i < 3; ++i )
             q[i] = joint->node[0].body->posr.pos[i]
                    - q[i]
                    - joint->node[1].body->posr.pos[i];

File gazebo/physics/BallJoint.hh

               {
               }
 
+      /// \internal
+      public: virtual unsigned int GetAngleCount() const
+              {
+                return 0;
+              }
+
       /// \brief Template to ::Load the BallJoint.
       /// \param[in] _sdf SDF to load the joint from.
       public: void Load(sdf::ElementPtr _sdf)
-              {T::Load(_sdf);}
+              {
+                T::Load(_sdf);
+              }
 
-      /// \internal
-      /// \brief Set the axis of rotation. This is not used for ball joints.
-      public: virtual void SetAxis(unsigned int /*_index*/,
-                                   const math::Vector3 &/*_axis*/) {}
 
-      /// \internal
-      /// \brief Set the high stop of an axis(index).
-      public: virtual bool SetHighStop(unsigned int /*_index*/,
-                                 const math::Angle &/*_angle*/) {return true;}
-
-      /// \internal
-      /// \brief Set the low stop of an axis(index).
-      public: virtual bool SetLowStop(unsigned int /*_index*/,
-                                 const math::Angle &/*_angle*/) {return true;}
-
-      /// \internal
-      /// \brief Get the high stop of an axis(index).
-      public: virtual math::Angle GetHighStop(unsigned int /*_index*/)
-              {return math::Angle();}
-
-      /// \internal
-      /// \brief Get the low stop of an axis(index).
-      public: virtual math::Angle GetLowStop(unsigned int /*_index*/)
-              {return math::Angle();}
-
-      /// \internal
-      public: virtual unsigned int GetAngleCount() const
-              {return 0;}
+      /// \brief Initialize joint
+      protected: virtual void Init()
+                 {
+                   T::Init();
+                 }
     };
     /// \}
   }

File gazebo/physics/Gripper.cc

File contents unchanged.

File gazebo/physics/Gripper.hh

File contents unchanged.

File gazebo/physics/Joint.cc

File contents unchanged.

File gazebo/physics/Joint.hh

       public: double GetDamping(unsigned int _index);
 
       /// \brief Callback to apply damping force to joint.
-      /// Deprecated by ApplyStiffnessDamping.
+      /// Deprecated by ApplySpringStiffnessDamping.
       public: virtual void ApplyDamping() GAZEBO_DEPRECATED(3.0);
 
       /// \brief Callback to apply spring stiffness and viscous damping
       /// effects to joint.
+      /// \TODO: rename to ApplySpringStiffnessDamping()
       public: virtual void ApplyStiffnessDamping();
 
       /// \brief Set the joint spring stiffness.
       ///                   implemented.
       /// \param[in] _stiffness Stiffness value for the axis.
       /// \param[in] _reference Spring zero load reference position.
+      /// \TODO: rename to SetSpringStiffnessDamping()
       public: virtual void SetStiffnessDamping(unsigned int _index,
         double _stiffness, double _damping, double _reference = 0) = 0;
 
       /// \param[in] _index Index of the axis to set, currently ignored, to be
       ///                   implemented.
       /// \param[in] _stiffness Spring stiffness value for the axis.
+      /// \TODO: rename to SetSpringStiffness()
       public: virtual void SetStiffness(unsigned int _index,
                                         double _stiffness) = 0;
 
       /// \param[in] _index Index of the axis to get, currently ignored, to be
       ///                   implemented.
       /// \return Joint spring stiffness coefficient for this joint.
+      /// \TODO: rename to GetSpringStiffness()
       public: double GetStiffness(unsigned int _index);
 
       /// \brief Get joint spring reference position.

File gazebo/physics/ScrewJoint.hh

                  }
 
       /// \brief Set screw joint thread pitch.
-      ///
+      /// Thread Pitch is defined as angular motion per linear
+      /// motion or rad / m in metric.
       /// This must be implemented in a child class
+      /// Deprecated, please use the index-less version in the future:
+      /// virtual void SetThreadPitch(double _threadPitch) = 0;
       /// \param[in] _index Index of the axis.
       /// \param[in] _threadPitch Thread pitch value.
       public: virtual void SetThreadPitch(unsigned int _index,
                   double _threadPitch) GAZEBO_DEPRECATED(3.0) = 0;
 
       /// \brief Set screw joint thread pitch.
-      ///
+      /// Thread Pitch is defined as angular motion per linear
+      /// motion or rad / m in metric.
       /// This must be implemented in a child class
-      /// \param[in] _index Index of the axis.
+      /// To clarify direction, these are modeling right handed threads
+      /// with positive thread_pitch, i.e. the child Link is the nut
+      /// (interior threads) while the parent Link is the bolt/screw
+      /// (exterior threads).
       /// \param[in] _threadPitch Thread pitch value.
       public: virtual void SetThreadPitch(double _threadPitch) = 0;
 
       /// \brief Get screw joint thread pitch.
-      ///
+      /// Thread Pitch is defined as angular motion per linear
+      /// motion or rad / m in metric.
       /// This must be implemented in a child class
       /// \param[in] _index Index of the axis.
       /// \return _threadPitch Thread pitch value.
         GAZEBO_DEPRECATED(3.0) = 0;
 
       /// \brief Get screw joint thread pitch.
-      ///
+      /// Thread Pitch is defined as angular motion per linear
+      /// motion or rad / m in metric.
       /// This must be implemented in a child class
-      /// \param[in] _index Index of the axis.
       /// \return _threadPitch Thread pitch value.
       public: virtual double GetThreadPitch() = 0;
 

File gazebo/physics/bullet/BulletBallJoint.cc

   return math::Vector3();
 }
 
+//////////////////////////////////////////////////
+void BulletBallJoint::SetAxis(unsigned int /*_index*/,
+                        const math::Vector3 &/*_axis*/)
+{
+  gzerr << "BulletBallJoint::SetAxis not implemented" << std::endl;
+}
+
+//////////////////////////////////////////////////
+math::Angle BulletBallJoint::GetHighStop(unsigned int /*_index*/)
+{
+  gzerr << "BulletBallJoint::GetHighStop not implemented" << std::endl;
+  return math::Angle();
+}
+
+//////////////////////////////////////////////////
+math::Angle BulletBallJoint::GetLowStop(unsigned int /*_index*/)
+{
+  gzerr << "BulletBallJoint::GetLowStop not implemented" << std::endl;
+  return math::Angle();
+}

File gazebo/physics/bullet/BulletBallJoint.hh

       // Documentation inherited.
       protected: virtual void SetForceImpl(unsigned int _index, double _torque);
 
-      /// \brief Pointer to Bullet ball constraint
+      // Documentation inherited.
+      public: virtual void SetAxis(unsigned int _index,
+                                   const math::Vector3 &_axis);
+
+      // Documentation inherited.
+      public: virtual math::Angle GetHighStop(unsigned int _index);
+
+      // Documentation inherited.
+      public: virtual math::Angle GetLowStop(unsigned int _index);
+
+      /// \brief bullet ball constraint
       private: btPoint2PointConstraint *bulletBall;
     };
 

File gazebo/physics/bullet/BulletJoint.cc

File contents unchanged.

File gazebo/physics/bullet/BulletJoint.hh

File contents unchanged.

File gazebo/physics/bullet/BulletScrewJoint.cc

  * limitations under the License.
  *
  */
-/* Desc: A bullet screw or primastic joint
- * Author: Nate Koenig
- * Date: 13 Oct 2009
- */
-
+#include <algorithm>
+#include <limits>
 #include <string>
 
 #include "gazebo/common/Assert.hh"
 #include "gazebo/physics/bullet/BulletTypes.hh"
 #include "gazebo/physics/bullet/BulletScrewJoint.hh"
 
+namespace gazebo
+{
+  namespace physics
+  {
+    class btScrewConstraint : public btSliderConstraint
+    {
+      public: btScrewConstraint(btRigidBody &_rbA, btRigidBody &_rbB,
+          const btTransform &_frameInA, const btTransform &_frameInB,
+          bool _useLinearReferenceFrameA)
+          : btSliderConstraint(_rbA, _rbB, _frameInA, _frameInB,
+              _useLinearReferenceFrameA), threadPitch(1.0) {}
+
+      public: btScrewConstraint(btRigidBody &_rbB,
+          const btTransform &_frameInB, bool _useLinearReferenceFrameA)
+          : btSliderConstraint(_rbB, _frameInB, _useLinearReferenceFrameA),
+          threadPitch(1.0) {}
+
+      public: virtual void getInfo1(btConstraintInfo1 *_info)
+      {
+        this->_getInfo1NonVirtual(_info);
+      }
+
+      public: virtual void getInfo2(btConstraintInfo2 *_info)
+      {
+        this->_getInfo2NonVirtual(
+            _info,
+            m_rbA.getCenterOfMassTransform(),
+            m_rbB.getCenterOfMassTransform(),
+            m_rbA.getLinearVelocity(),
+            m_rbB.getLinearVelocity(),
+            m_rbA.getInvMass(),
+            m_rbB.getInvMass());
+      }
+
+      public: void _getInfo1NonVirtual(btConstraintInfo1* info);
+      public: void _getInfo2NonVirtual(
+          btConstraintInfo2* info,
+          const btTransform& transA,
+          const btTransform& transB,
+          const btVector3& linVelA,
+          const btVector3& linVelB,
+          btScalar rbAinvMass, btScalar rbBinvMass);
+
+      public: btScalar getLinearPosition();
+      public: btScalar getAngularPosition();
+
+      // needed non-const version for SetForce
+      public: btRigidBody& getRigidBodyA();
+      public: btRigidBody& getRigidBodyB();
+
+      public: virtual void setThreadPitch(double _threadPitch)
+      {
+        this->threadPitch = -_threadPitch;
+      }
+
+      public: virtual double getThreadPitch() const
+      {
+        return -this->threadPitch;
+      }
+
+      private: double threadPitch;
+    };
+  }
+}
+
 using namespace gazebo;
 using namespace physics;
 
 {
   ScrewJoint<BulletJoint>::Init();
 
-  gzwarn << "Screw joint constraints are currently not enforced" << "\n";
-
   BulletLinkPtr bulletChildLink =
     boost::static_pointer_cast<BulletLink>(this->childLink);
   BulletLinkPtr bulletParentLink =
     boost::static_pointer_cast<BulletLink>(this->parentLink);
 
+  // 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);
+  }
 
-  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;
-  math::Pose pose;
+  // Initialize pivots to anchorPos, which is expressed in the
+  // world coordinate frame.
+  pivotParent = this->anchorPos;
+  pivotChild = this->anchorPos;
 
-  pivotA = this->anchorPos;
-  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.
-    pivotA -= pose.pos;
+    pivotParent -= pose.pos;
     // Rotate pivot offset and axis into body-fixed frame of parent.
-    pivotA = pose.rot.RotateVectorReverse(pivotA);
+    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.
-    pivotB -= pose.pos;
+    pivotChild -= pose.pos;
     // Rotate pivot offset and axis into body-fixed frame of child.
-    pivotB = pose.rot.RotateVectorReverse(pivotB);
+    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());
   }
 
-  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);
-
   // If both links exist, then create a joint between the two links.
   if (bulletChildLink && bulletParentLink)
   {
-    this->bulletScrew = new btSliderConstraint(
+    this->bulletScrew = new btScrewConstraint(
         *bulletParentLink->GetBulletLink(),
         *bulletChildLink->GetBulletLink(),
-        frame1, frame2, true);
+        frameParent, frameChild, 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);
+    this->bulletScrew = new btScrewConstraint(
+        *bulletChildLink->GetBulletLink(), frameChild, 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);
+    this->bulletScrew = new btScrewConstraint(
+        *bulletParentLink->GetBulletLink(), frameParent, true);
   }
   // Throw an error if no links are given.
   else
   {
-    gzthrow("joint without links\n");
+    gzerr << "Unable to create a joint without links\n";
+    return;
   }
 
+  if (!this->bulletScrew)
+  {
+    gzerr << "unable to create bullet screw joint\n";
+    return;
+  }
+
+  // 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");
+  // joint limit is set on the revolute dof in sdf,
+  double upper = limitElem->Get<double>("upper");
+  double lower = limitElem->Get<double>("lower");
+  this->bulletScrew->setLowerAngLimit(lower);
+  this->bulletScrew->setUpperAngLimit(upper);
+  // enforce linear dof in bullet.
+  double tp = this->threadPitch;
+  if (math::equal(tp, 0.0))
+  {
+    gzerr << "thread pitch should not be zero (joint is a slider?)"
+          << " using thread pitch = 1.0e6\n";
+    tp = 1.0e6;
+  }
+  if (tp > 0)
+  {
+    this->bulletScrew->setLowerLinLimit(lower/tp);
+    this->bulletScrew->setUpperLinLimit(upper/tp);
+  }
+  else
+  {
+    this->bulletScrew->setLowerLinLimit(upper/tp);
+    this->bulletScrew->setUpperLinLimit(lower/tp);
+  }
+  this->bulletScrew->setThreadPitch(tp);
+
   this->constraint = this->bulletScrew;
 
   // Add the joint to the world
 double BulletScrewJoint::GetVelocity(unsigned int /*_index*/) const
 {
   double result = 0;
-  if (this->bulletScrew)
-    result = this->bulletScrew->getTargetLinMotorVelocity();
+  math::Vector3 globalAxis = this->GetGlobalAxis(0);
+  if (this->childLink)
+    result += globalAxis.Dot(this->childLink->GetWorldLinearVel());
+  if (this->parentLink)
+    result -= globalAxis.Dot(this->parentLink->GetWorldLinearVel());
   return result;
 }
 
 //////////////////////////////////////////////////
-void BulletScrewJoint::SetVelocity(unsigned int /*_index*/, double _angle)
+void BulletScrewJoint::SetVelocity(unsigned int _index, double _vel)
 {
-  if (this->bulletScrew)
-    this->bulletScrew->setTargetLinMotorVelocity(_angle);
+  math::Vector3 desiredVel;
+  if (this->parentLink)
+    desiredVel = this->parentLink->GetWorldLinearVel();
+  desiredVel += _vel * this->GetGlobalAxis(_index);
+  if (this->childLink)
+    this->childLink->SetLinearVel(desiredVel);
 }
 
 //////////////////////////////////////////////////
 void BulletScrewJoint::SetAxis(unsigned int /*_index*/,
-    const math::Vector3 &/*_axis*/)
+    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->bulletScrew)
+  {
+    // this hasn't been initialized yet, store axis in initialWorldAxis
+    math::Quaternion axisFrame = this->GetAxisFrame(0);
+    this->initialWorldAxis = axisFrame.RotateVector(_axis);
+  }
+  else
+  {
+    gzerr << "SetAxis for existing joint is not implemented\n";
+  }
 }
 
 //////////////////////////////////////////////////
 void BulletScrewJoint::SetThreadPitch(unsigned int /*_index*/,
-    double /*_threadPitch*/)
+    double _threadPitch)
 {
-  gzerr << "Not implemented\n";
+  this->SetThreadPitch(_threadPitch);
 }
 
 //////////////////////////////////////////////////
-void BulletScrewJoint::SetThreadPitch(double /*_threadPitch*/)
+void BulletScrewJoint::SetThreadPitch(double _threadPitch)
 {
-  gzerr << "Not implemented\n";
+  this->threadPitch = _threadPitch;
+}
+
+//////////////////////////////////////////////////
+double BulletScrewJoint::GetThreadPitch()
+{
+  double result = this->threadPitch;
+  if (this->bulletScrew)
+    result = this->bulletScrew->getThreadPitch();
+  else
+    gzwarn << "bulletScrew not created yet, returning cached threadPitch.\n";
+  return result;
 }
 
 //////////////////////////////////////////////////
 double BulletScrewJoint::GetThreadPitch(unsigned int /*_index*/)
 {
-  return this->threadPitch;
+  return this->GetThreadPitch();
 }
 
 //////////////////////////////////////////////////
-double BulletScrewJoint::GetThreadPitch()
-{
-  return this->threadPitch;
-}
-
-//////////////////////////////////////////////////
-void BulletScrewJoint::SetForceImpl(unsigned int /*_index*/, double /*_force*/)
-{
-  gzlog << "Not implemented\n";
-}
-
-//////////////////////////////////////////////////
-bool BulletScrewJoint::SetHighStop(unsigned int /*_index*/,
-    const math::Angle &_angle)
+void BulletScrewJoint::SetForceImpl(unsigned int _index, double _force)
 {
   if (this->bulletScrew)
-    this->bulletScrew->setUpperLinLimit(_angle.Radian());
-  return true;
-}
+  {
+    // x-axis of constraint frame
+    btVector3 hingeAxisLocalA =
+      this->bulletScrew->getFrameOffsetA().getBasis().getColumn(0);
+    btVector3 hingeAxisLocalB =
+      this->bulletScrew->getFrameOffsetB().getBasis().getColumn(0);
 
-//////////////////////////////////////////////////
-bool BulletScrewJoint::SetLowStop(unsigned int /*_index*/,
-    const math::Angle &_angle)
-{
-  if (this->bulletScrew)
-    this->bulletScrew->setLowerLinLimit(_angle.Radian());
-  return true;
-}
+    btVector3 hingeAxisWorldA =
+      this->bulletScrew->getRigidBodyA().getWorldTransform().getBasis() *
+      hingeAxisLocalA;
+    btVector3 hingeAxisWorldB =
+      this->bulletScrew->getRigidBodyB().getWorldTransform().getBasis() *
+      hingeAxisLocalB;
 
-//////////////////////////////////////////////////
-math::Angle BulletScrewJoint::GetHighStop(unsigned int /*_index*/)
-{
-  math::Angle result;
-  if (this->bulletScrew)
-    result = this->bulletScrew->getUpperLinLimit();
-  return result;
-}
+    btVector3 hingeEffortA = _force * hingeAxisWorldA;
+    btVector3 hingeEffortB = _force * hingeAxisWorldB;
 
-//////////////////////////////////////////////////
-math::Angle BulletScrewJoint::GetLowStop(unsigned int /*_index*/)
-{
-  math::Angle result;
-  if (this->bulletScrew)
-    result = this->bulletScrew->getLowerLinLimit();
-  return result;
+    if (_index == 0)
+    {
+      this->bulletScrew->getRigidBodyA().applyTorque(-hingeEffortA);
+      this->bulletScrew->getRigidBodyB().applyTorque(hingeEffortB);
+    }
+    else if (_index == 1)
+    {
+      if (this->constraint)
+      {
+        // TODO: switch to applyForce and specify body-fixed offset
+        this->constraint->getRigidBodyA().applyCentralForce(-hingeEffortA);
+        this->constraint->getRigidBodyB().applyCentralForce(hingeEffortB);
+      }
+      else
+        gzerr << "BulletScrewJoint::constraint not created yet.\n";
+    }
+    else
+      gzerr << "Invalid index [" << _index << "]\n";
+  }
+  else
+    gzerr << "bulletScrew not created yet.\n";
 }
 
 //////////////////////////////////////////////////
 {
   if (this->bulletScrew)
     this->bulletScrew->setMaxLinMotorForce(_force);
+  else
+    gzerr << "bulletScrew not created yet\n";
 }
 
 //////////////////////////////////////////////////
   double result = 0;
   if (this->bulletScrew)
     result = this->bulletScrew->getMaxLinMotorForce();
+  else
+    gzerr << "bulletScrew not created yet\n";
   return result;
 }
 
 //////////////////////////////////////////////////
+bool BulletScrewJoint::SetHighStop(unsigned int _index,
+                      const math::Angle &_angle)
+{
+  Joint::SetHighStop(0, _angle);
+
+  // bulletScrew axial rotation is backward
+  if (!this->bulletScrew)
+  {
+    gzerr << "bulletScrew not created yet.\n";
+    return false;
+  }
+
+  if (_index == 0)
+  {
+    // _index = 0: angular constraint
+    double upperAng = this->bulletScrew->getUpperAngLimit();
+    this->bulletScrew->setUpperAngLimit(std::max(upperAng, _angle.Radian()));
+
+    // set corresponding linear constraints
+    double tp = this->threadPitch;
+    if (math::equal(tp, 0.0))
+    {
+      gzwarn << "thread pitch should not be zero (joint is a slider?)"
+             << " using thread pitch = 1.0e6\n";
+      tp = 1.0e6;
+    }
+    // linear is angular / threadPitch
+    if (tp > 0)
+    {
+      double lowerLin = this->bulletScrew->getLowerLinLimit();
+      this->bulletScrew->setUpperLinLimit(std::max(lowerLin,
+        _angle.Radian()/tp));
+    }
+    else
+    {
+      // flip upper lower because thread pitch is negative
+      double upperLin = this->bulletScrew->getUpperLinLimit();
+      this->bulletScrew->setLowerLinLimit(std::min(upperLin,
+        _angle.Radian()/tp));
+    }
+    return true;
+  }
+  else if (_index == 1)
+  {
+    // _index = 1: linear constraint
+    double lowerLin = this->bulletScrew->getLowerLinLimit();
+    this->bulletScrew->setUpperLinLimit(std::max(lowerLin, _angle.Radian()));
+
+    // set corresponding angular constraints
+    double tp = this->threadPitch;
+    // angular is linear * threadPitch
+    if (tp > 0)
+    {
+      double lowerAng = this->bulletScrew->getLowerAngLimit();
+      this->bulletScrew->setUpperAngLimit(std::max(lowerAng,
+        _angle.Radian()*tp));
+    }
+    else
+    {
+      // flip upper lower because thread pitch is negative
+      double upperAng = this->bulletScrew->getUpperAngLimit();
+      this->bulletScrew->setLowerAngLimit(std::min(upperAng,
+        _angle.Radian()*tp));
+    }
+    return true;
+  }
+  else
+  {
+    gzerr << "Invalid index [" << _index << "]\n";
+    return false;
+  }
+}
+
+//////////////////////////////////////////////////
+bool BulletScrewJoint::SetLowStop(unsigned int _index,
+                     const math::Angle &_angle)
+{
+  Joint::SetLowStop(0, _angle);
+
+  // bulletScrew axial rotation is backward
+  if (!this->bulletScrew)
+  {
+    gzerr << "bulletScrew not created yet.\n";
+    return false;
+  }
+
+  // bulletScrew axial rotation is backward
+  if (_index == 0)
+  {
+    // _index = 0: angular constraint
+    double upperAng = this->bulletScrew->getUpperAngLimit();
+    this->bulletScrew->setLowerAngLimit(std::min(upperAng, _angle.Radian()));
+
+    // set corresponding linear constraints
+    double tp = this->threadPitch;
+    if (math::equal(tp, 0.0))
+    {
+      gzerr << "thread pitch should not be zero (joint is a slider?)"
+            << " using thread pitch = 1.0e6\n";
+      tp = 1.0e6;
+    }
+    // linear is angular / threadPitch
+    if (tp > 0)
+    {
+      double upperLin = this->bulletScrew->getUpperLinLimit();
+      this->bulletScrew->setLowerLinLimit(std::min(upperLin,
+        _angle.Radian()/tp));
+    }
+    else
+    {
+        // flip upper lower because thread pitch is negative
+      double lowerLin = this->bulletScrew->getLowerLinLimit();
+      this->bulletScrew->setUpperLinLimit(std::max(lowerLin,
+        _angle.Radian()/tp));
+    }
+    return true;
+  }
+  else if (_index == 1)
+  {
+    // _index = 1: linear constraint
+    double upperLin = this->bulletScrew->getUpperLinLimit();
+    this->bulletScrew->setLowerLinLimit(std::min(upperLin, _angle.Radian()));
+
+    // set corresponding angular constraints
+    double tp = this->threadPitch;
+    // angular is linear * threadPitch
+    if (tp > 0)
+    {
+      double upperAng = this->bulletScrew->getUpperAngLimit();
+      this->bulletScrew->setLowerAngLimit(std::min(upperAng,
+        _angle.Radian()*tp));
+    }
+    else
+    {
+        // flip upper lower because thread pitch is negative
+      double lowerAng = this->bulletScrew->getLowerAngLimit();
+      this->bulletScrew->setUpperAngLimit(std::max(lowerAng,
+        _angle.Radian()*tp));
+    }
+    return true;
+  }
+  else
+  {
+    gzerr << "Invalid index [" << _index << "]\n";
+    return false;
+  }
+}
+
+//////////////////////////////////////////////////
 math::Vector3 BulletScrewJoint::GetGlobalAxis(unsigned int /*_index*/) const
 {
-  math::Vector3 result;
+  math::Vector3 result = this->initialWorldAxis;
   if (this->bulletScrew)
   {
-    // I have not verified the following math, though I based it on internal
-    // bullet code at line 250 of btHingeConstraint.cpp
+    // bullet uses x-axis for slider
     btVector3 vec =
-      this->bulletScrew->getRigidBodyA().getCenterOfMassTransform().getBasis() *
-      this->bulletScrew->getFrameOffsetA().getBasis().getColumn(2);
+      this->bulletScrew->getRigidBodyA().getCenterOfMassTransform().getBasis()
+      * this->bulletScrew->getFrameOffsetA().getBasis().getColumn(0);
     result = BulletTypes::ConvertVector3(vec);
   }
   else
-    gzwarn << "bulletHinge does not exist, returning fake axis\n";
+    gzwarn << "bulletScrew does not exist, returning fake axis\n";
   return result;
 }
 
 //////////////////////////////////////////////////
-math::Angle BulletScrewJoint::GetAngleImpl(unsigned int /*_index*/) const
+math::Angle BulletScrewJoint::GetAngleImpl(unsigned int _index) const
 {
   math::Angle result;
   if (this->bulletScrew)
-    result = this->bulletScrew->getLinearPos();
+  {
+    if (_index == 0)
+    {
+      // angular position
+      result = this->bulletScrew->getAngularPosition();
+    }
+    else if (_index == 1)
+    {
+      // linear position
+      result = this->bulletScrew->getLinearPosition();
+    }
+    else
+      gzerr << "Invalid index [" << _index << "]\n";
+  }
+  else
+    gzerr << "bulletScrew not created yet\n";
   return result;
 }
 
   else
     return BulletJoint::GetParam(_key, _index);
 }
+
+//////////////////////////////////////////////////
+btScalar btScrewConstraint::getAngularPosition()
+{
+  this->calculateTransforms(
+    m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
+  const btVector3 axisA0 = m_calculatedTransformA.getBasis().getColumn(1);
+  const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2);
+  const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
+  return btAtan2(axisB0.dot(axisA1), axisB0.dot(axisA0));
+}
+
+//////////////////////////////////////////////////
+btScalar btScrewConstraint::getLinearPosition()
+{
+  this->calculateTransforms(
+    m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
+  return this->m_depth[0];
+}
+
+//////////////////////////////////////////////////
+btRigidBody& btScrewConstraint::getRigidBodyA()
+{
+  return this->m_rbA;
+}
+
+//////////////////////////////////////////////////
+btRigidBody& btScrewConstraint::getRigidBodyB()
+{
+  return this->m_rbB;
+}
+
+//////////////////////////////////////////////////
+void btScrewConstraint::_getInfo2NonVirtual(
+    btConstraintInfo2* info,
+    const btTransform& transA,
+    const btTransform& transB,
+    const btVector3& linVelA,
+    const btVector3& linVelB,
+    btScalar rbAinvMass, btScalar rbBinvMass)
+{
+  /// This is a copy of btSliderConstraint::getInfo2NonVirtual(...)
+  /// with minor changes to the ax1 direction constraint.
+  /// Mainly, the axial limit constraint is always on and is
+  /// changed to a screw constraint.
+
+  /// First, always turn on
+  const btTransform& trA = getCalculatedTransformA();
+  const btTransform& trB = getCalculatedTransformB();
+
+  btAssert(!m_useSolveConstraintObsolete);
+  int i, s = info->rowskip;
+
+  btScalar signFact = m_useLinearReferenceFrameA ?
+    btScalar(1.0f) : btScalar(-1.0f);
+
+  // difference between frames in WCS
+  btVector3 ofs = trB.getOrigin() - trA.getOrigin();
+  // now get weight factors depending on masses
+  btScalar miA = rbAinvMass;
+  btScalar miB = rbBinvMass;
+  bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
+  btScalar miS = miA + miB;
+  btScalar factA, factB;
+  if (miS > btScalar(0.f))
+  {
+    factA = miB / miS;
+  }
+  else
+  {
+    factA = btScalar(0.5f);
+  }
+  factB = btScalar(1.0f) - factA;
+  btVector3 ax1, p, q;
+  btVector3 ax1A = trA.getBasis().getColumn(0);
+  btVector3 ax1B = trB.getBasis().getColumn(0);
+  if (m_useOffsetForConstraintFrame)
+  {
+    // get the desired direction of slider axis
+    // as weighted sum of X-orthos of frameA and frameB in WCS
+    ax1 = ax1A * factA + ax1B * factB;
+    ax1.normalize();
+    // construct two orthos to slider axis
+    btPlaneSpace1(ax1, p, q);
+  }
+  else
+  { // old way - use frameA
+    ax1 = trA.getBasis().getColumn(0);
+    // get 2 orthos to slider axis (Y, Z)
+    p = trA.getBasis().getColumn(1);
+    q = trA.getBasis().getColumn(2);
+  }
+  // make rotations around these orthos equal
+  // the slider axis should be the only unconstrained
+  // rotational axis, the angular velocity of the two bodies perpendicular to
+  // the slider axis should be equal. thus the constraint equations are
+  //    p*w1 - p*w2 = 0
+  //    q*w1 - q*w2 = 0
+  // where p and q are unit vectors normal to the slider axis, and w1 and w2
+  // are the angular velocity vectors of the two bodies.
+  info->m_J1angularAxis[0] = p[0];
+  info->m_J1angularAxis[1] = p[1];
+  info->m_J1angularAxis[2] = p[2];
+  info->m_J1angularAxis[s+0] = q[0];
+  info->m_J1angularAxis[s+1] = q[1];
+  info->m_J1angularAxis[s+2] = q[2];
+
+  info->m_J2angularAxis[0] = -p[0];
+  info->m_J2angularAxis[1] = -p[1];
+  info->m_J2angularAxis[2] = -p[2];
+  info->m_J2angularAxis[s+0] = -q[0];
+  info->m_J2angularAxis[s+1] = -q[1];
+  info->m_J2angularAxis[s+2] = -q[2];
+  // compute the right hand side of the constraint equation. set relative
+  // body velocities along p and q to bring the slider back into alignment.
+  // if ax1A,ax1B are the unit length slider axes as computed from bodyA and
+  // bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2).
+  // if "theta" is the angle between ax1 and ax2, we need an angular velocity
+  // along u to cover angle erp*theta in one step :
+  //   |angular_velocity| = angle/time = erp*theta / stepsize
+  //                      = (erp*fps) * theta
+  //    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
+  //                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
+  // ...as ax1 and ax2 are unit length. if theta is smallish,
+  // theta ~= sin(theta), so
+  //    angular_velocity  = (erp*fps) * (ax1 x ax2)
+  // ax1 x ax2 is in the plane space of ax1, so we project the angular
+  // velocity to p and q to find the right hand side.
+//  btScalar k = info->fps * info->erp * getSoftnessOrthoAng();
+  btScalar currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTANG) ?
+    m_softnessOrthoAng : m_softnessOrthoAng * info->erp;
+  btScalar k = info->fps * currERP;
+
+  btVector3 u = ax1A.cross(ax1B);
+  info->m_constraintError[0] = k * u.dot(p);
+  info->m_constraintError[s] = k * u.dot(q);
+  if (m_flags & BT_SLIDER_FLAGS_CFM_ORTANG)
+  {
+    info->cfm[0] = m_cfmOrthoAng;
+    info->cfm[s] = m_cfmOrthoAng;
+  }
+
+  // last filled row
+  int nrow = 1;
+  int srow;
+  btScalar limit_err;
+  int limit;
+  int powered;
+
+  // next two rows.
+  // we want: velA + wA x relA == velB + wB x relB ... but this would
+  // result in three equations, so we project along two orthos to the
+  // slider axis
+
+  btTransform bodyA_trans = transA;
+  btTransform bodyB_trans = transB;
+  nrow++;
+  int s2 = nrow * s;
+  nrow++;
+  int s3 = nrow * s;
+  btVector3 tmpA(0, 0, 0), tmpB(0, 0, 0), relA(0, 0, 0),
+    relB(0, 0, 0), c(0, 0, 0);
+  if (m_useOffsetForConstraintFrame)
+  {
+    // get vector from bodyB to frameB in WCS
+    relB = trB.getOrigin() - bodyB_trans.getOrigin();
+    // get its projection to slider axis
+    btVector3 projB = ax1 * relB.dot(ax1);
+    // get vector directed from bodyB to slider axis (and orthogonal to it)
+    btVector3 orthoB = relB - projB;
+    // same for bodyA
+    relA = trA.getOrigin() - bodyA_trans.getOrigin();
+    btVector3 projA = ax1 * relA.dot(ax1);
+    btVector3 orthoA = relA - projA;
+    // get desired offset between frames A and B along slider axis
+    btScalar sliderOffs = m_linPos - m_depth[0];
+    // desired vector from projection of center of bodyA to projection of
+    // center of bodyB to slider axis
+    btVector3 totalDist = projA + ax1 * sliderOffs - projB;
+    // get offset vectors relA and relB
+    relA = orthoA + totalDist * factA;
+    relB = orthoB - totalDist * factB;
+    // now choose average ortho to slider axis
+    p = orthoB * factA + orthoA * factB;
+    btScalar len2 = p.length2();
+    if (len2 > SIMD_EPSILON)
+    {
+      p /= btSqrt(len2);
+    }
+    else
+    {
+      p = trA.getBasis().getColumn(1);
+    }
+    // make one more ortho
+    q = ax1.cross(p);
+    // fill two rows
+    tmpA = relA.cross(p);
+    tmpB = relB.cross(p);
+    for (i = 0; i < 3; ++i)
+    {
+      info->m_J1angularAxis[s2+i] = tmpA[i];
+      info->m_J2angularAxis[s2+i] = -tmpB[i];
+    }
+    tmpA = relA.cross(q);
+    tmpB = relB.cross(q);
+    if (hasStaticBody && getSolveAngLimit())
+    { // to make constraint between static and dynamic objects more rigid
+      // remove wA (or wB) from equation if angular limit is hit
+      tmpB *= factB;
+      tmpA *= factA;
+    }
+    for (i = 0; i < 3; ++i)
+    {
+      info->m_J1angularAxis[s3+i] = tmpA[i];
+      info->m_J2angularAxis[s3+i] = -tmpB[i];
+      info->m_J1linearAxis[s2+i] = p[i];
+      info->m_J1linearAxis[s3+i] = q[i];
+      info->m_J2linearAxis[s2+i] = -p[i];
+      info->m_J2linearAxis[s3+i] = -q[i];
+    }
+  }
+  else
+  {
+    // old way - maybe incorrect if bodies are not on the slider axis
+    // see discussion "Bug in slider constraint"
+    // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=9&t=4024&start=0
+    c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin();
+    btVector3 tmp = c.cross(p);
+    for (i = 0; i < 3; ++i)
+    {
+     info->m_J1angularAxis[s2+i] = factA*tmp[i];
+     info->m_J2angularAxis[s2+i] = factB*tmp[i];
+    }
+    tmp = c.cross(q);
+    for (i = 0; i < 3; ++i)
+    {
+      info->m_J1angularAxis[s3+i] = factA*tmp[i];
+      info->m_J2angularAxis[s3+i] = factB*tmp[i];
+      info->m_J1linearAxis[s2+i] = p[i];
+      info->m_J1linearAxis[s3+i] = q[i];
+      info->m_J2linearAxis[s2+i] = -p[i];
+      info->m_J2linearAxis[s3+i] = -q[i];
+    }
+  }
+  // compute two elements of right hand side
+
+  //  k = info->fps * info->erp * getSoftnessOrthoLin();
+  currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN) ?
+    m_softnessOrthoLin : m_softnessOrthoLin * info->erp;
+  k = info->fps * currERP;
+
+  btScalar rhs = k * p.dot(ofs);
+  info->m_constraintError[s2] = rhs;
+  rhs = k * q.dot(ofs);
+  info->m_constraintError[s3] = rhs;
+  if (m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN)
+  {
+    info->cfm[s2] = m_cfmOrthoLin;
+    info->cfm[s3] = m_cfmOrthoLin;
+  }
+
+  // Screw Constraint (coupled linear and angular motion)
+  {
+    nrow++;
+    srow = nrow * info->rowskip;
+    info->m_J1linearAxis[srow+0] = -ax1[0] * this->threadPitch;
+    info->m_J1linearAxis[srow+1] = -ax1[1] * this->threadPitch;
+    info->m_J1linearAxis[srow+2] = -ax1[2] * this->threadPitch;
+
+    info->m_J1angularAxis[srow+0] = ax1[0];
+    info->m_J1angularAxis[srow+1] = ax1[1];
+    info->m_J1angularAxis[srow+2] = ax1[2];
+
+    info->m_J2linearAxis[srow+0] = ax1[0] * this->threadPitch;
+    info->m_J2linearAxis[srow+1] = ax1[1] * this->threadPitch;
+    info->m_J2linearAxis[srow+2] = ax1[2] * this->threadPitch;
+
+    info->m_J2angularAxis[srow+0] = -ax1[0];
+    info->m_J2angularAxis[srow+1] = -ax1[1];
+    info->m_J2angularAxis[srow+2] = -ax1[2];
+
+    // correction
+    // rhs = k * ax1.dot(ofs);  // from hinge constraint
+    btScalar lin_disp = ax1.dot(ofs);
+    btScalar ang_pos = this->getAngularPosition();
+    info->m_constraintError[srow] =
+      -k * (lin_disp * this->threadPitch - ang_pos);
+    info->cfm[srow] = -m_cfmOrthoLin;
+
+    // debug, set cfm to 0
+    // info->cfm[srow] = 0;
+    // debug, set error correction to 0
+    // info->m_constraintError[srow] = 0.0;
+  }
+
+  // notes: below enforces
+  // joint limit
+  // powered joint
+  // rotation along slider axis
+
+  // check linear limits
+  limit_err = btScalar(0.0);
+  limit = 0;
+  if (getSolveLinLimit())
+  {
+    limit_err = getLinDepth() *  signFact;
+    limit = (limit_err > btScalar(0.0)) ? 2 : 1;
+  }
+  powered = 0;
+  if (getPoweredLinMotor())
+  {
+    powered = 1;
+  }
+  // if the slider has joint limits or motor, add in the extra row
+  if (limit || powered)
+  {
+    nrow++;
+    srow = nrow * info->rowskip;
+    info->m_J1linearAxis[srow+0] = ax1[0];
+    info->m_J1linearAxis[srow+1] = ax1[1];
+    info->m_J1linearAxis[srow+2] = ax1[2];
+    info->m_J2linearAxis[srow+0] = -ax1[0];
+    info->m_J2linearAxis[srow+1] = -ax1[1];
+    info->m_J2linearAxis[srow+2] = -ax1[2];
+    // linear torque decoupling step:
+    //
+    // we have to be careful that the linear constraint forces (+/- ax1)
+    // applied to the two bodies
+    // do not create a torque couple. in other words, the points that the
+    // constraint force is applied at must lie along the same ax1 axis.
+    // a torque couple will result in limited slider-jointed free
+    // bodies from gaining angular momentum.
+    if (m_useOffsetForConstraintFrame)
+    {
+      // this is needed only when bodyA and bodyB are both dynamic.
+      if (!hasStaticBody)
+      {
+        tmpA = relA.cross(ax1);
+        tmpB = relB.cross(ax1);
+        info->m_J1angularAxis[srow+0] = tmpA[0];
+        info->m_J1angularAxis[srow+1] = tmpA[1];
+        info->m_J1angularAxis[srow+2] = tmpA[2];
+        info->m_J2angularAxis[srow+0] = -tmpB[0];
+        info->m_J2angularAxis[srow+1] = -tmpB[1];
+        info->m_J2angularAxis[srow+2] = -tmpB[2];
+      }
+    }
+    else
+    { // The old way. May be incorrect if bodies are not on the slider axis
+      // Linear Torque Decoupling vector (a torque)
+      btVector3 ltd;
+      ltd = c.cross(ax1);
+      info->m_J1angularAxis[srow+0] = factA*ltd[0];
+      info->m_J1angularAxis[srow+1] = factA*ltd[1];
+      info->m_J1angularAxis[srow+2] = factA*ltd[2];
+      info->m_J2angularAxis[srow+0] = factB*ltd[0];
+      info->m_J2angularAxis[srow+1] = factB*ltd[1];
+      info->m_J2angularAxis[srow+2] = factB*ltd[2];
+    }
+    // right-hand part
+    btScalar lostop = getLowerLinLimit();
+    btScalar histop = getUpperLinLimit();
+
+    // issue #1104:
+    // if (limit && (lostop == histop)) raises warnings, using
+    // a warning-less implementation.
+    if (limit &&
+      math::equal(lostop, histop,
+      static_cast<btScalar>(std::numeric_limits<double>::epsilon())))
+    {
+      // the joint motor is ineffective
+      powered = 0;
+    }
+    info->m_constraintError[srow] = 0.;
+    info->m_lowerLimit[srow] = 0.;
+    info->m_upperLimit[srow] = 0.;
+    currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN) ?
+      m_softnessLimLin : info->erp;
+    if (powered)
+    {
+      if (m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN)
+      {
+        info->cfm[srow] = m_cfmDirLin;
+      }
+      btScalar tag_vel = getTargetLinMotorVelocity();
+      btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit,
+        m_upperLinLimit, tag_vel, info->fps * currERP);
+      info->m_constraintError[srow] -=
+        signFact * mot_fact * getTargetLinMotorVelocity();
+      info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps;
+      info->m_upperLimit[srow] += getMaxLinMotorForce() * info->fps;
+    }
+    if (limit)
+    {
+      k = info->fps * currERP;
+      info->m_constraintError[srow] += k * limit_err;
+      if (m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN)
+      {
+        info->cfm[srow] = m_cfmLimLin;
+      }
+      // issue #1104:
+      // if (lostop == histop) raises warnings, using
+      // a warning-less implementation.
+      if (math::equal(lostop, histop,
+          static_cast<btScalar>(std::numeric_limits<double>::epsilon())))
+      {
+        // limited low and high simultaneously
+        info->m_lowerLimit[srow] = -SIMD_INFINITY;
+        info->m_upperLimit[srow] = SIMD_INFINITY;
+      }
+      else if (limit == 1)
+      {
+         // low limit
+        info->m_lowerLimit[srow] = -SIMD_INFINITY;
+        info->m_upperLimit[srow] = 0;
+      }
+      else
+      { // high limit
+        info->m_lowerLimit[srow] = 0;
+        info->m_upperLimit[srow] = SIMD_INFINITY;
+      }
+      // bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin)
+      //   for that)
+      btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin());
+      if (bounce > btScalar(0.0))
+      {
+        btScalar vel = linVelA.dot(ax1);
+        vel -= linVelB.dot(ax1);
+        vel *= signFact;
+        // only apply bounce if the velocity is incoming, and if the
+        // resulting c[] exceeds what we already have.
+        if (limit == 1)
+        {  // low limit
+          if (vel < 0)
+          {
+            btScalar newc = -bounce * vel;
+            if (newc > info->m_constraintError[srow])
+            {
+              info->m_constraintError[srow] = newc;
+            }
+          }
+        }
+        else
+        { // high limit - all those computations are reversed
+          if (vel > 0)
+          {
+            btScalar newc = -bounce * vel;
+            if (newc < info->m_constraintError[srow])
+            {
+              info->m_constraintError[srow] = newc;
+            }
+          }
+        }
+      }
+      info->m_constraintError[srow] *= getSoftnessLimLin();
+    }
+    // line above is the end of if (limit)
+  }
+  // line above is the end of if linear limit
+
+  // printf("tp: %f\n", this->threadPitch);
+
+  // check angular limits
+  limit_err = btScalar(0.0);
+  limit = 0;
+  if (getSolveAngLimit())
+  {
+    limit_err = getAngDepth();
+    limit = (limit_err > btScalar(0.0)) ? 1 : 2;
+  }
+  // if the slider has joint limits, add in the extra row
+  powered = 0;
+  if (getPoweredAngMotor())
+  {
+    powered = 1;
+  }
+  if (limit || powered)
+  {
+    nrow++;
+    srow = nrow * info->rowskip;
+    info->m_J1angularAxis[srow+0] = ax1[0];
+    info->m_J1angularAxis[srow+1] = ax1[1];
+    info->m_J1angularAxis[srow+2] = ax1[2];
+
+    info->m_J2angularAxis[srow+0] = -ax1[0];
+    info->m_J2angularAxis[srow+1] = -ax1[1];
+    info->m_J2angularAxis[srow+2] = -ax1[2];
+
+    btScalar lostop = getLowerAngLimit();
+    btScalar histop = getUpperAngLimit();
+    // issue #1104:
+    // if (limit && (lostop == histop)) raises warnings, using
+    // a warning-less implementation.
+    if (limit &&
+      math::equal(lostop, histop,
+      static_cast<btScalar>(std::numeric_limits<double>::epsilon())))
+    {  // the joint motor is ineffective
+      powered = 0;
+    }
+    currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMANG) ?
+      m_softnessLimAng : info->erp;
+    if (powered)
+    {
+      if (m_flags & BT_SLIDER_FLAGS_CFM_DIRANG)
+      {
+        info->cfm[srow] = m_cfmDirAng;
+      }
+      btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit,
+        m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * currERP);
+      info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity();
+      info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps;
+      info->m_upperLimit[srow] = getMaxAngMotorForce() * info->fps;
+    }
+    if (limit)
+    {
+      k = info->fps * currERP;
+      info->m_constraintError[srow] += k * limit_err;
+      if (m_flags & BT_SLIDER_FLAGS_CFM_LIMANG)
+      {
+        info->cfm[srow] = m_cfmLimAng;
+      }
+      // issue #1104:
+      // if (lostop == histop) raises warnings, using
+      // a warning-less implementation.
+      if (math::equal(lostop, histop,
+          static_cast<btScalar>(std::numeric_limits<double>::epsilon())))
+      {
+        // limited low and high simultaneously
+        info->m_lowerLimit[srow] = -SIMD_INFINITY;
+        info->m_upperLimit[srow] = SIMD_INFINITY;
+      }
+      else if (limit == 1)
+      { // low limit
+        info->m_lowerLimit[srow] = 0;
+        info->m_upperLimit[srow] = SIMD_INFINITY;
+      }
+      else
+      { // high limit
+        info->m_lowerLimit[srow] = -SIMD_INFINITY;
+        info->m_upperLimit[srow] = 0;
+      }
+      // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng)
+      // for that)
+      btScalar bounce = btFabs(btScalar(1.0) - getDampingLimAng());
+      if (bounce > btScalar(0.0))
+      {
+        btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
+        vel -= m_rbB.getAngularVelocity().dot(ax1);
+        // only apply bounce if the velocity is incoming, and if the
+        // resulting c[] exceeds what we already have.
+        if (limit == 1)
+        {  // low limit
+          if (vel < 0)
+          {
+            btScalar newc = -bounce * vel;
+            if (newc > info->m_constraintError[srow])
+            {
+              info->m_constraintError[srow] = newc;
+            }
+          }
+        }
+        else
+        {  // high limit - all those computations are reversed
+          if (vel > 0)
+          {
+            btScalar newc = -bounce * vel;
+            if (newc < info->m_constraintError[srow])
+            {
+              info->m_constraintError[srow] = newc;
+            }
+          }
+        }
+      }
+      info->m_constraintError[srow] *= getSoftnessLimAng();
+    }
+    // line above is the end of if (limit)
+  }
+  // line above is the end of if angular limit or powered
+}
+
+//////////////////////////////////////////////////
+void btScrewConstraint::_getInfo1NonVirtual(btConstraintInfo1* info)
+{
+  /// this is a modified version of
+  /// void btSliderConstraint::getInfo1(btConstraintInfo1* info)
+  /// with the rotational limits always turned on.
+  /// The rotational limit constraint is modified to be a screw constraint
+  /// in btScrewConstraint::_getInfo2NonVirtual
+  /// which is a copy of btSliderConstraint::getInfo2NonVirtual.
+
+  // info->m_numConstraintRows = 6;
+  // Fixed 2 linear + 2 angular + 1 limit (even if not used)
+  // info->nub = 0;
+
+  if (m_useSolveConstraintObsolete)
+  {
+    info->m_numConstraintRows = 0;
+    info->nub = 0;
+  }
+  else
+  {
+    // Fixed 2 linear + 2 angular
+    info->m_numConstraintRows = 4;
+    info->nub = 2;
+
+    // Add constraint for screw motion, coupling linear and angular motion.
+    info->m_numConstraintRows++;
+    info->nub++;
+
+    // prepare constraint
+    calculateTransforms(
+      m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
+    testLinLimits();
+    if (getSolveLinLimit() || getPoweredLinMotor())
+    {
+      // limit 3rd linear as well
+      info->m_numConstraintRows++;
+      info->nub--;
+    }
+    testAngLimits();
+    if (getSolveAngLimit() || getPoweredAngMotor())
+    {
+      // limit 3rd angular as well
+      info->m_numConstraintRows++;
+      info->nub--;
+    }
+  }
+  // printf("m: %d\n", info->m_numConstraintRows);
+}

File gazebo/physics/bullet/BulletScrewJoint.hh

 #include "gazebo/physics/ScrewJoint.hh"
 #include "gazebo/util/system.hh"
 
-class btSliderConstraint;
-
 namespace gazebo
 {
   namespace physics
   {
+    class btScrewConstraint;
+
     /// \ingroup gazebo_physics
     /// \addtogroup gazebo_physics_bullet Bullet Physics
     /// \{
 
       /// \brief Load the BulletScrewJoint
       /// \param[in] _sdf SDF values.
-      protected: virtual void Load(sdf::ElementPtr _sdf);
+      public: virtual void Load(sdf::ElementPtr _sdf);
+
+      // Documentation inherited
+      public: virtual math::Vector3 GetAnchor(unsigned int _index) const;
+
+      // Documentation inherited
+      public: virtual void SetAnchor(unsigned int _index,
+                  const math::Vector3 &_anchor);
 
       // Documentation inherited.
       public: virtual void Init();
                   double _threadPitch);
 
       // Documentation inherited
+      public: virtual void SetThreadPitch(double _threadPitch);
+
+      // Documentation inherited
       public: virtual double GetThreadPitch(unsigned int _index);
 
+      // Documentation inherited
+      public: virtual double GetThreadPitch();
+
       // Documentation inherited.
       public: virtual bool SetHighStop(unsigned int _index,
                   const math::Angle &_angle);
       public: virtual bool SetLowStop(unsigned int _index,
                   const math::Angle &_angle);
 
-      /// \brief Get the high stop of an axis(index).
-      public: virtual math::Angle GetHighStop(unsigned int _index);
-
-      /// \brief Get the low stop of an axis(index).
-      public: virtual math::Angle GetLowStop(unsigned int _index);
-
       /// \brief Get the rate of change
       /// \param[in] _index Axis index.
       public: virtual double GetVelocity(unsigned int _index) const;
                   unsigned int _index) GAZEBO_DEPRECATED(3.0);
 
       // Documentation inherited.
-      public: virtual math::Angle GetAngleImpl(unsigned int _index) const;
+      protected: virtual math::Angle GetAngleImpl(unsigned int _index) const;
 
       // Documentation inherited.
       protected: virtual void SetForceImpl(unsigned int _index, double _force);
 
       /// \brief Pointer to bullet screw constraint
-      private: btSliderConstraint *bulletScrew;
+      private: btScrewConstraint *bulletScrew;
+
+      /// \brief Initial value of joint axis, expressed as unit vector
+      ///        in world frame.
+      private: math::Vector3 initialWorldAxis;
     };
     /// \}
   }

File gazebo/physics/dart/DARTBallJoint.cc

 //////////////////////////////////////////////////
 double DARTBallJoint::GetVelocity(unsigned int /*_index*/) const
 {
+  gzerr << "DARTBallJoint::GetVelocity not implemented" << std::endl;
   return 0;
 }
 
 //////////////////////////////////////////////////
 math::Angle DARTBallJoint::GetAngleImpl(unsigned int /*_index*/) const
 {
+  gzerr << "DARTBallJoint::GetAngleImpl not implemented" << std::endl;
   return math::Angle(0);
 }
 
 //////////////////////////////////////////////////
 void DARTBallJoint::SetForceImpl(unsigned int /*_index*/, double /*_torque*/)
 {
-  gzerr << "Not implemented";
+  gzerr << "DARTBallJoint::SetForceImpl not implemented";
 }
+
+//////////////////////////////////////////////////
+void DARTBallJoint::SetAxis(unsigned int /*_index*/,
+                            const math::Vector3 &/*_axis*/)
+{
+  gzerr << "DARTBallJoint::SetAxis not implemented" << std::endl;
+}
+
+//////////////////////////////////////////////////
+math::Angle DARTBallJoint::GetHighStop(unsigned int /*_index*/)
+{
+  gzerr << "DARTBallJoint::GetHighStop not implemented" << std::endl;
+  return math::Angle();
+}
+
+//////////////////////////////////////////////////
+math::Angle DARTBallJoint::GetLowStop(unsigned int /*_index*/)
+{
+  gzerr << "DARTBallJoint::GetLowStop not implemented" << std::endl;
+  return math::Angle();
+}
+
+//////////////////////////////////////////////////
+bool DARTBallJoint::SetHighStop(unsigned int /*_index*/,
+                                const math::Angle &/*_angle*/)
+{
+  gzerr << "DARTBallJoint::SetHighStop not implemented" << std::endl;
+  return false;
+}
+
+//////////////////////////////////////////////////
+bool DARTBallJoint::SetLowStop(unsigned int /*_index*/,
+                               const math::Angle &/*_angle*/)
+{
+  gzerr << "DARTBallJoint::SetLowStop not implemented" << std::endl;
+  return false;
+}

File gazebo/physics/dart/DARTBallJoint.hh

       // Documentation inherited.
       protected: void SetForceImpl(unsigned int _index, double _torque);
 
+      // Documentation inherited.
+      public: virtual void SetAxis(unsigned int _index,
+                                   const math::Vector3 &_axis);
+
+      // Documentation inherited.
+      public: virtual math::Angle GetHighStop(unsigned int _index);
+
+      // Documentation inherited.
+      public: virtual math::Angle GetLowStop(unsigned int _index);
+
+      // Documentation inherited.
+      public: virtual bool SetHighStop(unsigned int _index,
+                                       const math::Angle &_angle);
+
+      // Documentation inherited.
+      public: virtual bool SetLowStop(unsigned int _index,
+                                      const math::Angle &_angle);
+
       /// \brief
       protected: dart::dynamics::BallJoint *dtBallJoint;
     };

File gazebo/physics/dart/DARTJoint.cc

File contents unchanged.

File gazebo/physics/dart/DARTJoint.hh

File contents unchanged.

File gazebo/physics/dart/DARTScrewJoint.cc

 }
 
 //////////////////////////////////////////////////
+math::Vector3 DARTScrewJoint::GetAnchor(unsigned int /*index*/) const
+{
+  gzerr << "DARTScrewJoint::GetAnchor not implemented, return 0 vector.\n";
+  return math::Vector3();
+}
+
+//////////////////////////////////////////////////
+void DARTScrewJoint::SetAnchor(unsigned int /*index*/,
+    const math::Vector3 &/*_anchor*/)
+{
+  gzerr << "DARTScrewJoint::SetAnchor not implemented.\n";
+}
+
+//////////////////////////////////////////////////
 void DARTScrewJoint::Init()
 {
   ScrewJoint<DARTJoint>::Init();
 }
 
 //////////////////////////////////////////////////
+void DARTScrewJoint::SetThreadPitch(double _threadPitch)
+{
+  this->SetThreadPitch(0, _threadPitch);
+}
+
+//////////////////////////////////////////////////
 double DARTScrewJoint::GetThreadPitch(unsigned int _index)
 {
   if (_index != 0)
 }
 
 //////////////////////////////////////////////////
+double DARTScrewJoint::GetThreadPitch()
+{
+  return this->GetThreadPitch(0);
+}
+
+//////////////////////////////////////////////////