Commits

Jeongseok Lee  committed eb77a9b Draft Merge

Merge with gazebo_3.0_dart_4.0

  • Participants
  • Parent commits 6b580f0, 4703306
  • Branches gazebo_3.0_dart_4.0_atlas_test

Comments (0)

Files changed (42)

File cmake/SearchForStuff.cmake

File contents unchanged.

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

File contents unchanged.

File gazebo/Server.hh

File contents unchanged.

File gazebo/physics/Joint.cc

File contents unchanged.

File gazebo/physics/Joint.hh

File contents unchanged.

File gazebo/physics/World.cc

File contents unchanged.

File gazebo/physics/bullet/BulletJoint.cc

File contents unchanged.

File gazebo/physics/bullet/BulletJoint.hh

File contents unchanged.

File gazebo/physics/bullet/BulletScrewJoint.cc

File contents unchanged.

File gazebo/physics/bullet/BulletScrewJoint.hh

File contents unchanged.

File gazebo/physics/dart/DARTHinge2Joint.cc

 
   if (_index == 0)
   {
-    double radianAngle = this->dtJoint->getGenCoord(0)->get_q();
+    double radianAngle = this->dtJoint->getGenCoord(0)->getConfig();
     result.SetFromRadian(radianAngle);
   }
   else if (_index == 1)
   {
-    double radianAngle = this->dtJoint->getGenCoord(1)->get_q();
+    double radianAngle = this->dtJoint->getGenCoord(1)->getConfig();
     result.SetFromRadian(radianAngle);
   }
   else
   double result = 0.0;
 
   if (_index == 0)
-    result = this->dtJoint->getGenCoord(0)->get_dq();
+    result = this->dtJoint->getGenCoord(0)->getVel();
   else if (_index == 1)
-    result = this->dtJoint->getGenCoord(1)->get_dq();
+    result = this->dtJoint->getGenCoord(1)->getVel();
   else
     gzerr << "Invalid index[" << _index << "]\n";
 
 void DARTHinge2Joint::SetVelocity(unsigned int _index, double _vel)
 {
   if (_index == 0)
-    this->dtJoint->getGenCoord(0)->set_dq(_vel);
+    this->dtJoint->getGenCoord(0)->setVel(_vel);
   else if (_index == 1)
-    this->dtJoint->getGenCoord(1)->set_dq(_vel);
+    this->dtJoint->getGenCoord(1)->setVel(_vel);
   else
     gzerr << "Invalid index[" << _index << "]\n";
 }
   double result = 0.0;
 
   if (_index == 0)
-    result = this->dtJoint->getGenCoord(0)->get_tauMax();
+    result = this->dtJoint->getGenCoord(0)->getForceMax();
   else if (_index == 1)
-    result = this->dtJoint->getGenCoord(1)->get_tauMax();
+    result = this->dtJoint->getGenCoord(1)->getForceMax();
   else
     gzerr << "Invalid index[" << _index << "]\n";
 
 void DARTHinge2Joint::SetMaxForce(unsigned int _index, double _force)
 {
   if (_index == 0)
-    this->dtJoint->getGenCoord(0)->set_tauMax(_force);
+    this->dtJoint->getGenCoord(0)->setForceMax(_force);
   else if (_index == 1)
-    this->dtJoint->getGenCoord(1)->set_tauMax(_force);
+    this->dtJoint->getGenCoord(1)->setForceMax(_force);
   else
     gzerr << "Invalid index[" << _index << "]\n";
 }
 void DARTHinge2Joint::SetForceImpl(unsigned int _index, double _effort)
 {
   if (_index == 0)
-    this->dtJoint->getGenCoord(0)->set_tau(_effort);
+    this->dtJoint->getGenCoord(0)->setForce(_effort);
   else if (_index == 1)
-    this->dtJoint->getGenCoord(1)->set_tau(_effort);
+    this->dtJoint->getGenCoord(1)->setForce(_effort);
   else
     gzerr << "Invalid index[" << _index << "]\n";
 }

File gazebo/physics/dart/DARTHingeJoint.cc

 
   if (_index == 0)
   {
-    double radianAngle = this->dtJoint->getGenCoord(0)->get_q();
+    double radianAngle = this->dtJoint->getGenCoord(0)->getConfig();
     result.SetFromRadian(radianAngle);
   }
   else
 void DARTHingeJoint::SetVelocity(unsigned int _index, double _vel)
 {
   if (_index == 0)
-    this->dtJoint->getGenCoord(0)->set_dq(_vel);
+    this->dtJoint->getGenCoord(0)->setVel(_vel);
   else
     gzerr << "Invalid index[" << _index << "]\n";
 }
   double result = 0.0;
 
   if (_index == 0)
-    result = this->dtJoint->getGenCoord(0)->get_dq();
+    result = this->dtJoint->getGenCoord(0)->getVel();
   else
     gzerr << "Invalid index[" << _index << "]\n";
 
 void DARTHingeJoint::SetMaxForce(unsigned int _index, double _force)
 {
   if (_index == 0)
-    this->dtJoint->getGenCoord(0)->set_tauMax(_force);
+    this->dtJoint->getGenCoord(0)->setForceMax(_force);
   else
     gzerr << "Invalid index[" << _index << "]\n";
 }
   double result = 0.0;
 
   if (_index == 0)
-    result = this->dtJoint->getGenCoord(0)->get_tauMax();
+    result = this->dtJoint->getGenCoord(0)->getForceMax();
   else
     gzerr << "Invalid index[" << _index << "]\n";
 
 void DARTHingeJoint::SetForceImpl(unsigned int _index, double _effort)
 {
   if (_index == 0)
-    this->dtJoint->getGenCoord(0)->set_tau(_effort);
+    this->dtJoint->getGenCoord(0)->setForce(_effort);
   else
     gzerr << "Invalid index[" << _index << "]\n";
 }

File gazebo/physics/dart/DARTJoint.cc

 {
   this->dartPhysicsEngine = boost::dynamic_pointer_cast<DARTPhysics>(
                               this->GetWorld()->GetPhysicsEngine());
-
+  this->stiffnessDampingInitialized = false;
   this->forceApplied[0] = 0.0;
   this->forceApplied[1] = 0.0;
 }
 //////////////////////////////////////////////////
 void DARTJoint::SetDamping(unsigned int _index, double _damping)
 {
-  this->dampingCoefficient = _damping;
-
-  if (this->GetAngleCount() > 2)
+  if (_index < this->GetAngleCount())
   {
-     gzerr << "Incompatible joint type, GetAngleCount() = "
-           << this->GetAngleCount() << " > 2\n";
-     return;
+    this->SetStiffnessDamping(
+          _index, this->stiffnessCoefficient[_index], _damping);
   }
-
-  // \TODO: implement on a per axis basis (requires additional sdf parameters)
-
-  /// \TODO:  this check might not be needed?  attaching an object to a static
-  /// body should not affect damping application.
-  bool parentStatic = this->GetParent() ? this->GetParent()->IsStatic() : false;
-  bool childStatic = this->GetChild() ? this->GetChild()->IsStatic() : false;
-
-  if (!parentStatic && !childStatic)
+  else
   {
-    this->dtJoint->setDampingCoefficient(_index, _damping);
+    gzerr << "DARTJoint::SetDamping: index[" << _index
+          << "] is out of bounds (GetAngleCount() = "
+          << this->GetAngleCount() << ").\n";
+    return;
   }
 }
 
     this->dissipationCoefficient[_index] = _damping;
     this->springReferencePosition[_index] = _reference;
 
-    /// \TODO: set joint stiffness coefficient
+    /// \TODO: this check might not be needed?  attaching an object to a static
+    /// body should not affect damping application.
+    bool parentStatic =
+        this->GetParent() ? this->GetParent()->IsStatic() : false;
+    bool childStatic =
+        this->GetChild() ? this->GetChild()->IsStatic() : false;
 
-    /// setting joint damping
-    bool parentStatic = this->GetParent() ?
-      this->GetParent()->IsStatic() : false;
-    bool childStatic = this->GetChild() ? this->GetChild()->IsStatic() : false;
-
-    if (!parentStatic && !childStatic)
+    if (!this->stiffnessDampingInitialized)
     {
-      this->dtJoint->setDampingCoefficient(_index, _damping);
+      if (!parentStatic && !childStatic)
+      {
+        this->dtJoint->setSpringStiffness(
+              static_cast<int>(_index), _stiffness);
+        this->dtJoint->setRestPosition(
+              static_cast<int>(_index), _reference);
+        this->dtJoint->setDampingCoefficient(
+              static_cast<int>(_index), _damping);
+        this->applyDamping = physics::Joint::ConnectJointUpdate(
+          boost::bind(&DARTJoint::ApplyStiffnessDamping, this));
+        this->stiffnessDampingInitialized = true;
+      }
+      else
+      {
+        gzwarn << "Spring Damper for Joint[" << this->GetName()
+               << "] is not initialized because either parent[" << parentStatic
+               << "] or child[" << childStatic << "] is static.\n";
+      }
     }
-
-    /// \TODO: add spring force element
-    gzdbg << "Joint [" << this->GetName()
-           << "] stiffness not implement in DART\n";
   }
   else
-    gzerr << "SetStiffnessDamping _index " << _index << " is too large.\n";
+  {
+    gzerr << "SetStiffnessDamping _index too large.\n";
+  }
 }
 
 //////////////////////////////////////////////////
     case 0:
     case 1:
     case 2:
-      this->dtJoint->getGenCoord(_index)->set_qMax(_angle.Radian());
+      this->dtJoint->getGenCoord(_index)->setConfigMax(_angle.Radian());
       return true;
     default:
       gzerr << "Invalid index[" << _index << "]\n";
   case 0:
   case 1:
   case 2:
-    this->dtJoint->getGenCoord(_index)->set_qMin(_angle.Radian());
+    this->dtJoint->getGenCoord(_index)->setConfigMin(_angle.Radian());
     return true;
   default:
     gzerr << "Invalid index[" << _index << "]\n";
   case 0:
   case 1:
   case 2:
-    return this->dtJoint->getGenCoord(_index)->get_qMax();
+    return this->dtJoint->getGenCoord(_index)->getConfigMax();
   default:
     gzerr << "Invalid index[" << _index << "]\n";
   };
   case 0:
   case 1:
   case 2:
-    return this->dtJoint->getGenCoord(_index)->get_qMin();
+    return this->dtJoint->getGenCoord(_index)->getConfigMin();
   default:
     gzerr << "Invalid index[" << _index << "]\n";
   };
 /////////////////////////////////////////////////
 void DARTJoint::ApplyStiffnessDamping()
 {
-  // Stiffness and damping force is automatically applied by DART.
+  // DART applies stiffness and damping force implicitly itself by setting
+  // the stiffness coefficient and the damping coefficient using
+  // dart::dynamics::Joint::setSpringStiffness(index, stiffnessCoeff) and
+  // dart::dynamics::Joint::setDampingCoefficient(index, dampingCoeff).
+  // Therefore, we do nothing here.
 }
 
 /////////////////////////////////////////////////

File gazebo/physics/dart/DARTJoint.hh

File contents unchanged.

File gazebo/physics/dart/DARTLink.cc

  *
 */
 
+#include "gazebo/common/Assert.hh"
 #include "gazebo/common/Console.hh"
 #include "gazebo/common/Exception.hh"
 
   if (joint == NULL)
     return;
 
-  if (joint->getJointType() == dart::dynamics::Joint::FREE)
+  dart::dynamics::FreeJoint* freeJoint =
+      dynamic_cast<dart::dynamics::FreeJoint*>(joint);
+  if (freeJoint)
   {
-    dart::dynamics::FreeJoint* freeJoint =
-        dynamic_cast<dart::dynamics::FreeJoint*>(joint);
-
+    // If the parent joint is free joint, set the 6 dof to fit the target pose.
     const Eigen::Isometry3d &W = DARTTypes::ConvPose(this->GetWorldPose());
     const Eigen::Isometry3d &T1 = joint->getTransformFromParentBodyNode();
     const Eigen::Isometry3d &InvT2 = joint->getTransformFromChildBodyNode();
       P = this->dtBodyNode->getParentBodyNode()->getWorldTransform();
 
     Eigen::Isometry3d Q = T1.inverse() * P.inverse() * W * InvT2;
-    Eigen::Vector6d t = Eigen::Vector6d::Zero();
-    t.tail<3>() = Q.translation();
-    t.head<3>() = dart::math::logMap(Q.linear());
-    freeJoint->set_q(t);
+    // Set generalized coordinate and update the transformations only
+    freeJoint->setConfigs(dart::math::logMap(Q), true, false, false);
+  }
+  else if (joint->getNumGenCoords() > 0)
+  {
+    // If the parent joint is not free joint (nor weld joint), set the n dof
+    // as minimal value that makes the link's pose close to the target pose.
+    const Eigen::Isometry3d &W = DARTTypes::ConvPose(this->GetWorldPose());
+    this->dtBodyNode->fitWorldTransform(W);
   }
   else
   {
-    gzdbg << "DARTLink::OnPoseChange() doesn't make sense unless the link has "
-          << "free joint.\n";
+    gzdbg << "DARTLink::OnPoseChange() doesn't make sense if the dof of the "
+          << "parent joint is 0.\n";
   }
 }
 
 }
 
 //////////////////////////////////////////////////
-void DARTLink::SetLinearVel(const math::Vector3 &/*_vel*/)
+void DARTLink::SetLinearVel(const math::Vector3 &_vel)
 {
-  gzdbg << "DARTLink::SetLinearVel() doesn't make sense in dart.\n";
+  dtBodyNode->fitWorldLinearVel(DARTTypes::ConvVec3(_vel));
 }
 
 //////////////////////////////////////////////////
-void DARTLink::SetAngularVel(const math::Vector3 &/*_vel*/)
+void DARTLink::SetAngularVel(const math::Vector3 &_vel)
 {
-  gzdbg << "DARTLink::SetAngularVel() doesn't make sense in dart.\n";
+  dtBodyNode->fitWorldAngularVel(DARTTypes::ConvVec3(_vel));
 }
 
 //////////////////////////////////////////////////
 //////////////////////////////////////////////////
 math::Vector3 DARTLink::GetWorldCoGLinearVel() const
 {
-  Eigen::Vector3d worldCOM = this->dtBodyNode->getWorldCOM();
+  Eigen::Vector3d localCOM = this->dtBodyNode->getLocalCOM();
   Eigen::Vector3d linVel
-    = this->dtBodyNode->getWorldVelocity(worldCOM).tail<3>();
+    = this->dtBodyNode->getWorldVelocity(localCOM, true).tail<3>();
 
   return DARTTypes::ConvVec3(linVel);
 }
   Link_V links = this->GetModel()->GetLinks();
 
   bool isSkeletonSelfCollidable =
-      this->dtBodyNode->getSkeleton()->isSelfCollidable();
+      this->dtBodyNode->getSkeleton()->isEnabledSelfCollisionCheck();
 
   if (_collide)
   {
     // the pairs of which both of the links in the pair is not self collidable.
     else
     {
-      dtSkeleton->setSelfCollidable(true);
+      dtSkeleton->enableSelfCollision();
 
       for (size_t i = 0; i < links.size() - 1; ++i)
       {
       }
     }
     if (isAllLinksNotCollidable)
-      dtSkeleton->setSelfCollidable(false);
+      dtSkeleton->disableSelfCollision();
   }
 }
 

File gazebo/physics/dart/DARTModel.cc

   // collidable.
   if (hasPairOfSelfCollidableLinks)
   {
-    this->dtSkeleton->setSelfCollidable(true);
+    this->dtSkeleton->enableSelfCollision();
 
     dart::simulation::SoftWorld *dtWorld = this->GetDARTPhysics()->GetDARTWorld();
     dart::collision::CollisionDetector *dtCollDet =
 //////////////////////////////////////////////////
 void DARTModel::BackupState()
 {
-  dtConfig = this->dtSkeleton->get_q();
-  dtVelocity = this->dtSkeleton->get_dq();
+  dtConfig = this->dtSkeleton->getConfigs();
+  dtVelocity = this->dtSkeleton->getGenVels();
 }
 
 //////////////////////////////////////////////////
   GZ_ASSERT(dtVelocity.size() == this->dtSkeleton->getNumGenCoords(),
             "Cannot RestoreState, invalid size");
 
-  this->dtSkeleton->set_q(dtConfig);
-  this->dtSkeleton->set_dq(dtVelocity);
+  this->dtSkeleton->setConfigs(dtConfig, true, false, false);
+  this->dtSkeleton->setGenVels(dtVelocity, true, false);
 }
 
 //////////////////////////////////////////////////

File gazebo/physics/dart/DARTModel.hh

File contents unchanged.

File gazebo/physics/dart/DARTPhysics.cc

 
   // Gravity
   math::Vector3 g = this->sdf->Get<math::Vector3>("gravity");
+  // ODEPhysics checks this, so we will too.
+  if (g == math::Vector3(0, 0, 0))
+    gzwarn << "Gravity vector is (0, 0, 0). Objects will float.\n";
   this->dtWorld->setGravity(Eigen::Vector3d(g.x, g.y, g.z));
 
   // Time step
   }
   else
   {
-    gzwarn << _key << " is not supported in ode" << std::endl;
+    gzwarn << _key << " is not supported in dart" << std::endl;
     return 0;
   }
 

File gazebo/physics/dart/DARTPhysics.hh

File contents unchanged.

File gazebo/physics/dart/DARTScrewJoint.cc

   this->SetThreadPitch(0, this->threadPitch);
 }
 
-//////////////////////////////////////////////////
-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*/)
 {
 }
 
 //////////////////////////////////////////////////
+math::Vector3 DARTScrewJoint::GetAnchor(unsigned int /*index*/) const
+{
+  Eigen::Isometry3d T = this->dtChildBodyNode->getWorldTransform() *
+                        this->dtJoint->getTransformFromChildBodyNode();
+  Eigen::Vector3d worldOrigin = T.translation();
+
+  return DARTTypes::ConvVec3(worldOrigin);
+}
+
+//////////////////////////////////////////////////
 math::Vector3 DARTScrewJoint::GetGlobalAxis(unsigned int _index) const
 {
   Eigen::Vector3d globalAxis = Eigen::Vector3d::UnitX();
 
-  if (_index == 0)
+  if (_index == 0 || _index == 1)
   {
     Eigen::Isometry3d T = this->dtChildBodyNode->getWorldTransform() *
                           this->dtJoint->getTransformFromChildBodyNode();
 }
 
 //////////////////////////////////////////////////
+double DARTScrewJoint::GetAttribute(const std::string &_key,
+                                    unsigned int _index)
+{
+  if (_key  == "thread_pitch")
+    return this->threadPitch;
+  else
+    return DARTJoint::GetAttribute(_key, _index);
+}
+
+//////////////////////////////////////////////////
 double DARTScrewJoint::GetVelocity(unsigned int _index) const
 {
   double result = 0.0;
 
   if (_index == 0)
-    result = this->dtJoint->getGenCoord(0)->get_dq();
+    result = this->dtJoint->getGenCoord(0)->getVel();
   else
     gzerr << "Invalid index[" << _index << "]\n";
 
 void DARTScrewJoint::SetVelocity(unsigned int _index, double _vel)
 {
   if (_index == 0)
-    this->dtJoint->getGenCoord(0)->set_dq(_vel);
+    this->dtJoint->getGenCoord(0)->setVel(_vel);
   else
     gzerr << "Invalid index[" << _index << "]\n";
 }
 //////////////////////////////////////////////////
 void DARTScrewJoint::SetThreadPitch(unsigned int _index, double _threadPitch)
 {
-  if (_index == 0)
-    this->dartScrewJoint->setPitch(_threadPitch);
-  else
+  if (_index != 0)
     gzerr << "Invalid index[" << _index << "]\n";
+  this->SetThreadPitch(_threadPitch);
 }
 
 //////////////////////////////////////////////////
 void DARTScrewJoint::SetThreadPitch(double _threadPitch)
 {
-  this->SetThreadPitch(0, _threadPitch);
+  this->threadPitch = _threadPitch;
+  this->dartScrewJoint->setPitch(_threadPitch * 2.0 * M_PI);
 }
 
 //////////////////////////////////////////////////
 double DARTScrewJoint::GetThreadPitch(unsigned int _index)
 {
-  double result = 0.0;
-
-  if (_index == 0)
-    result = this->dartScrewJoint->getPitch();
-  else
+  if (_index != 0)
     gzerr << "Invalid index[" << _index << "]\n";
-
-  return result;
+  return this->GetThreadPitch();
 }
 
 //////////////////////////////////////////////////
 double DARTScrewJoint::GetThreadPitch()
 {
-  return this->GetThreadPitch(0);
+  double result = this->threadPitch;
+  if (this->dartScrewJoint)
+    result = this->dartScrewJoint->getPitch() * 0.5 / M_PI;
+  else
+    gzwarn << "dartScrewJoint not created yet, returning cached threadPitch.\n";
+  return result;
 }
 
 //////////////////////////////////////////////////
 {
   math::Angle result;
 
-  if (_index == 0)
+  if (this->dartScrewJoint)
   {
-    double radianAngle = this->dtJoint->getGenCoord(0)->get_q();
-    result.SetFromRadian(radianAngle);
+    if (_index == 0)
+    {
+      // angular position
+      result.SetFromRadian(this->dartScrewJoint->getGenCoord(0)->getConfig());
+    }
+    else if (_index == 1)
+    {
+      // linear position
+      double angPos = this->dartScrewJoint->getGenCoord(0)->getConfig();
+      result = dartScrewJoint->getPitch() * angPos * 0.5 / M_PI;
+    }
+    else
+    {
+      gzerr << "Invalid index[" << _index << "]\n";
+    }
   }
   else
   {
-    gzerr << "Invalid index[" << _index << "]\n";
+    gzerr << "dartScrewJoint not created yet\n";
   }
 
   return result;
 void DARTScrewJoint::SetMaxForce(unsigned int _index, double _force)
 {
   if (_index == 0)
-    this->dtJoint->getGenCoord(0)->set_tauMax(_force);
+    this->dtJoint->getGenCoord(0)->setForceMax(_force);
   else
     gzerr << "Invalid index[" << _index << "]\n";
 }
   double result = 0.0;
 
   if (_index == 0)
-    result = this->dtJoint->getGenCoord(0)->get_tauMax();
+    result = this->dtJoint->getGenCoord(0)->getForceMax();
   else
     gzerr << "Invalid index[" << _index << "]\n";
 
 void DARTScrewJoint::SetForceImpl(unsigned int _index, double _effort)
 {
   if (_index == 0)
-    this->dtJoint->getGenCoord(0)->set_tau(_effort);
+    this->dtJoint->getGenCoord(0)->setForce(_effort);
   else
     gzerr << "Invalid index[" << _index << "]\n";
 }
   switch (_index)
   {
   case 0:
-    return this->dtJoint->getGenCoord(_index)->get_qMax();
+    return this->dtJoint->getGenCoord(_index)->getConfigMax();
   default:
     gzerr << "Invalid index[" << _index << "]\n";
   };
   switch (_index)
   {
   case 0:
-    return this->dtJoint->getGenCoord(_index)->get_qMin();
+    return this->dtJoint->getGenCoord(_index)->getConfigMin();
   default:
     gzerr << "Invalid index[" << _index << "]\n";
   };

File gazebo/physics/dart/DARTScrewJoint.hh

       public: virtual void SetAxis(unsigned int _index,
                   const math::Vector3 &_axis);
 
+      // Documentation inherited.
+      public: virtual double GetAttribute(const std::string &_key,
+                  unsigned int _index) GAZEBO_DEPRECATED(3.0);
+
       // Documentation inherited
       public: virtual void SetThreadPitch(unsigned int _index,
                   double _threadPitch);

File gazebo/physics/dart/DARTSliderJoint.cc

 
   if (_index == 0)
   {
-    double radianAngle = this->dtJoint->getGenCoord(0)->get_q();
+    double radianAngle = this->dtJoint->getGenCoord(0)->getConfig();
     result.SetFromRadian(radianAngle);
   }
   else
 void DARTSliderJoint::SetVelocity(unsigned int _index, double _vel)
 {
   if (_index == 0)
-    this->dtJoint->getGenCoord(0)->set_dq(_vel);
+    this->dtJoint->getGenCoord(0)->setVel(_vel);
   else
     gzerr << "Invalid index[" << _index << "]\n";
 }
   double result = 0.0;
 
   if (_index == 0)
-    result = this->dtJoint->getGenCoord(0)->get_dq();
+    result = this->dtJoint->getGenCoord(0)->getVel();
   else
     gzerr << "Invalid index[" << _index << "]\n";
 
 void DARTSliderJoint::SetMaxForce(unsigned int _index, double _force)
 {
   if (_index == 0)
-    this->dtJoint->getGenCoord(0)->set_tauMax(_force);
+    this->dtJoint->getGenCoord(0)->setForceMax(_force);
   else
     gzerr << "Invalid index[" << _index << "]\n";
 }
   double result = 0.0;
 
   if (_index == 0)
-    result = this->dtJoint->getGenCoord(0)->get_tauMax();
+    result = this->dtJoint->getGenCoord(0)->getForceMax();
   else
     gzerr << "Invalid index[" << _index << "]\n";
 
 void DARTSliderJoint::SetForceImpl(unsigned int _index, double _effort)
 {
   if (_index == 0)
-    this->dtJoint->getGenCoord(0)->set_tau(_effort);
+    this->dtJoint->getGenCoord(0)->setForce(_effort);
   else
     gzerr << "Invalid index[" << _index << "]\n";
 }

File gazebo/physics/dart/DARTUniversalJoint.cc

 
   if (_index == 0)
   {
-    double radianAngle = this->dtJoint->getGenCoord(0)->get_q();
+    double radianAngle = this->dtJoint->getGenCoord(0)->getConfig();
     result.SetFromRadian(radianAngle);
   }
   else if (_index == 1)
   {
-    double radianAngle = this->dtJoint->getGenCoord(1)->get_q();
+    double radianAngle = this->dtJoint->getGenCoord(1)->getConfig();
     result.SetFromRadian(radianAngle);
   }
   else
   double result = 0.0;
 
   if (_index == 0)
-    result = this->dtJoint->getGenCoord(0)->get_dq();
+    result = this->dtJoint->getGenCoord(0)->getVel();
   else if (_index == 1)
-    result = this->dtJoint->getGenCoord(1)->get_dq();
+    result = this->dtJoint->getGenCoord(1)->getVel();
   else
     gzerr << "Invalid index[" << _index << "]\n";
 
 void DARTUniversalJoint::SetVelocity(unsigned int _index, double _vel)
 {
   if (_index == 0)
-    this->dtJoint->getGenCoord(0)->set_dq(_vel);
+    this->dtJoint->getGenCoord(0)->setVel(_vel);
   else if (_index == 1)
-    this->dtJoint->getGenCoord(1)->set_dq(_vel);
+    this->dtJoint->getGenCoord(1)->setVel(_vel);
   else
     gzerr << "Invalid index[" << _index << "]\n";
 }
 void DARTUniversalJoint::SetMaxForce(unsigned int _index, double _force)
 {
   if (_index == 0)
-    this->dtJoint->getGenCoord(0)->set_tauMax(_force);
+    this->dtJoint->getGenCoord(0)->setForceMax(_force);
   else if (_index == 1)
-    this->dtJoint->getGenCoord(1)->set_tauMax(_force);
+    this->dtJoint->getGenCoord(1)->setForceMax(_force);
   else
     gzerr << "Invalid index[" << _index << "]\n";
 }
   double result = 0.0;
 
   if (_index == 0)
-    result = this->dtJoint->getGenCoord(0)->get_tauMax();
+    result = this->dtJoint->getGenCoord(0)->getForceMax();
   else if (_index == 1)
-    result = this->dtJoint->getGenCoord(1)->get_tauMax();
+    result = this->dtJoint->getGenCoord(1)->getForceMax();
   else
     gzerr << "Invalid index[" << _index << "]\n";
 
 void DARTUniversalJoint::SetForceImpl(unsigned int _index, double _effort)
 {
   if (_index == 0)
-    this->dtJoint->getGenCoord(0)->set_tau(_effort);
+    this->dtJoint->getGenCoord(0)->setForce(_effort);
   else if (_index == 1)
-    this->dtJoint->getGenCoord(1)->set_tau(_effort);
+    this->dtJoint->getGenCoord(1)->setForce(_effort);
   else
     gzerr << "Invalid index[" << _index << "]\n";
 }

File gazebo/physics/ode/ODEJoint.cc

File contents unchanged.

File gazebo/physics/ode/ODEScrewJoint.cc

File contents unchanged.

File gazebo/physics/ode/ODEScrewJoint.hh

File contents unchanged.

File gazebo/physics/ode/ODEUniversalJoint.cc

File contents unchanged.

File gazebo/physics/simbody/SimbodyJoint.cc

File contents unchanged.

File gazebo/physics/simbody/SimbodyJoint.hh

File contents unchanged.

File gazebo/physics/simbody/SimbodyLink.cc

File contents unchanged.

File gazebo/physics/simbody/SimbodyPhysics.cc

File contents unchanged.

File gazebo/physics/simbody/SimbodyScrewJoint.cc

File contents unchanged.

File gazebo/physics/simbody/SimbodyScrewJoint.hh

File contents unchanged.

File test/integration/CMakeLists.txt

File contents unchanged.

File test/integration/contact_sensor.cc

           contacts.contact(i).wrench(j).body_2_wrench().torque().z();
       }
 
-      // dart doesn't pass this portion of the test (#910)
-      if (_physicsEngine != "dart")
-      {
-        // contact sensor should have positive x torque and relatively large
-        // compared to y and z
-        EXPECT_GT(actualTorque.x, 0);
-        EXPECT_GT(actualTorque.x, fabs(actualTorque.y));
-        EXPECT_GT(actualTorque.x, fabs(actualTorque.z));
-        // EXPECT_LT(fabs(actualTorque.y), tol);
-        // EXPECT_LT(fabs(actualTorque.z), tol);
-      }
+      // contact sensor should have positive x torque and relatively large
+      // compared to y and z
+      EXPECT_GT(actualTorque.x, 0);
+      EXPECT_GT(actualTorque.x, fabs(actualTorque.y));
+      EXPECT_GT(actualTorque.x, fabs(actualTorque.z));
+      // EXPECT_LT(fabs(actualTorque.y), tol);
+      // EXPECT_LT(fabs(actualTorque.z), tol);
     }
   }
 }

File test/integration/joint_screw.cc

File contents unchanged.

File test/integration/joint_test.cc

File contents unchanged.

File test/integration/joint_universal.cc

File contents unchanged.

File test/integration/physics.cc

File contents unchanged.

File test/integration/physics_link.cc

           << std::endl;
     return;
   }
-  if (_physicsEngine == "dart")
-  {
-    gzerr << "DARTLink::SetLinearVel, SetAngularVel aren't working (#1079)"
-          << std::endl;
-    return;
-  }
 
   Load("worlds/empty.world", true, _physicsEngine);
   physics::WorldPtr world = physics::get_world("default");

File test/integration/physics_msgs.cc

File contents unchanged.

File worlds/CMakeLists.txt

File contents unchanged.