Commits

John Hsu committed ea45a41

update per pull request comments

  • Participants
  • Parent commits e1efc3e
  • Branches joint_effort_limit

Comments (0)

Files changed (9)

gazebo/physics/Joint.cc

 //////////////////////////////////////////////////
 double Joint::GetEffortLimit(int _index)
 {
-  if (_index < this->GetAngleCount())
+  if (_index >= 0 && _index < this->GetAngleCount())
     return this->effortLimit[_index];
 
   gzerr << "GetEffortLimit index[" << _index << "] out of range\n";
 //////////////////////////////////////////////////
 double Joint::GetVelocityLimit(int _index)
 {
-  if (_index < this->GetAngleCount())
+  if (_index >= 0 && _index < this->GetAngleCount())
     return this->velocityLimit[_index];
 
   gzerr << "GetVelocityLimit index[" << _index << "] out of range\n";
 //////////////////////////////////////////////////
 void Joint::SetForce(int _index, double _force)
 {
-  /// \todo: should check to see if this type of joint has _index
-  if (_index < 2)
+  // this bit of code actually doesn't do anything physical,
+  // it simply records the forces commanded inside forceApplied.
+  if (_index >= 0 && _index < this->GetAngleCount())
     this->forceApplied[_index] = _force;
   else
-    gzerr << "Invalid joint index [" << _index
-          << "] when trying to apply force\n";
+    gzerr << "Something's wrong, joint [" << this->GetName()
+          << "] index [" << _index
+          << "] out of range.\n";
 }
 
 //////////////////////////////////////////////////
 double Joint::GetForce(int _index)
 {
-  /// \todo: should check to see if this type of joint has _index
-  if (_index < 2)
+  if (_index >= 0 && _index < this->GetAngleCount())
+  {
     return this->forceApplied[_index];
+  }
   else
   {
     gzerr << "Invalid joint index [" << _index

gazebo/physics/Joint.hh

       public: virtual void SetForce(int _index, double _force);
 
       /// \brief @todo: not yet implemented.
-      /// Get the internal forces at a this Joint.
+      /// Get the forces applied at this Joint.
       /// Note that the unit of force should be consistent with the rest
       /// of the simulation scales.  E.g.  if you are using
       /// metric units, the unit for force is Newtons.  If using
       /// \return The force applied to an axis.
       public: virtual double GetForce(int _index);
 
-      /// \brief get force torque values at a joint
+      /// \brief get internal force and torque values at a joint
+      /// Note that you must set
+      ///   <provide_feedback>true<provide_feedback>
+      /// in the joint sdf to use this.
       /// \param[in] _index Force and torque on child link if _index = 0
       /// and on parent link of _index = 1
       /// \return The force and torque at the joint

gazebo/physics/bullet/BulletHingeJoint.cc

 //////////////////////////////////////////////////
 void BulletHingeJoint::SetForce(int _index, double _torque)
 {
+  if (_index < 0 || _index >= this->GetAngleCount())
+  {
+    gzerr << "Calling BulletHingeJoint::SetForce with an index ["
+          << _index << "] out of range\n";
+    return;
+  }
+
+  // truncate effort unless effortLimit is negative.
+  if (this->effortLimit[_index] >= 0)
+    _torque = math::clamp(_torque, -this->effortLimit[_index],
+       this->effortLimit[_index]);
+
   // math::Vector3 axis = this->GetLocalAxis(_index);
   // this->bulletHinge->enableAngularMotor(true);
 
     this->bulletHinge->getRigidBodyA().getWorldTransform().getBasis() *
     hingeAxisLocal;
 
-  if (_index < this->GetAngleCount() && this->effortLimit[_index] > 0)
-    _torque = math::clamp(_torque, -this->effortLimit[_index],
-       this->effortLimit[_index]);
-
   btVector3 hingeTorque = _torque * hingeAxisWorld;
 
   this->bulletHinge->getRigidBodyA().applyTorque(hingeTorque);

gazebo/physics/bullet/BulletSliderJoint.cc

 //////////////////////////////////////////////////
 void BulletSliderJoint::SetForce(int _index, double _force)
 {
+  if (_index < 0 || _index >= this->GetAngleCount())
+  {
+    gzerr << "Calling BulletSliderJoint::SetForce with an index ["
+          << _index << "] out of range\n";
+    return;
+  }
+
+  // truncate effort if effortLimit is not negative
+  if (this->effortLimit[_index] >= 0)
+    _force = math::clamp(_force, -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;
 
   btVector3 hingeTorque = _torque * hingeAxisWorld;
   */
 
-  if (_index < this->GetAngleCount() && this->effortLimit[_index] > 0)
-    _force = math::clamp(_force, -this->effortLimit[_index],
-       this->effortLimit[_index]);
-
   btVector3 force(0, 0, _force);
   this->constraint->getRigidBodyA().applyCentralForce(force);
   this->constraint->getRigidBodyB().applyCentralForce(-force);

gazebo/physics/ode/ODEHinge2Joint.cc

 //////////////////////////////////////////////////
 void ODEHinge2Joint::SetForce(int _index, double _torque)
 {
+  if (_index < 0 || _index >= this->GetAngleCount())
+  {
+    gzerr << "Calling BulletHingeJoint::SetForce with an index ["
+          << _index << "] out of range\n";
+    return;
+  }
+
+  // truncate effort if effortLimit is not negative
+  if (this->effortLimit[_index] >= 0)
+    _torque = math::clamp(_torque,
+      -this->effortLimit[_index], this->effortLimit[_index]);
+
   ODEJoint::SetForce(_index, _torque);
   if (this->childLink)
     this->childLink->SetEnabled(true);
   if (this->parentLink)
     this->parentLink->SetEnabled(true);
 
-  if (_index < this->GetAngleCount() && this->effortLimit[_index] > 0)
-    _torque = math::clamp(_torque,
-      -this->effortLimit[_index], this->effortLimit[_index]);
-
   if (_index == 0)
     dJointAddHinge2Torques(this->jointId, _torque, 0);
   else

gazebo/physics/ode/ODEHingeJoint.cc

 //////////////////////////////////////////////////
 void ODEHingeJoint::SetForce(int _index, double _torque)
 {
+  if (_index < 0 || _index >= this->GetAngleCount())
+  {
+    gzerr << "Calling ODEHingeJoint::SetForce with an index ["
+          << _index << "] out of range\n";
+    return;
+  }
+
+  // truncate effort if effortLimit is not negative
+  if (this->effortLimit[_index] >= 0)
+    _torque = math::clamp(_torque,
+      -this->effortLimit[_index], this->effortLimit[_index]);
+
   ODEJoint::SetForce(_index, _torque);
   if (this->childLink)
     this->childLink->SetEnabled(true);
   if (this->parentLink)
     this->parentLink->SetEnabled(true);
 
-  if (_index < this->GetAngleCount() && this->effortLimit[_index] > 0)
-    _torque = math::clamp(_torque,
-      -this->effortLimit[_index], this->effortLimit[_index]);
-
   dJointAddHingeTorque(this->jointId, _torque);
 }
 

gazebo/physics/ode/ODEScrewJoint.cc

 //////////////////////////////////////////////////
 void ODEScrewJoint::SetForce(int _index, double _force)
 {
+  if (_index < 0 || _index >= this->GetAngleCount())
+  {
+    gzerr << "Calling ODEScrewJoint::SetForce with an index ["
+          << _index << "] out of range\n";
+    return;
+  }
+
+  // truncate effort if effortLimit is not negative
+  if (this->effortLimit[_index] >= 0.0)
+    _force = math::clamp(_force,
+      -this->effortLimit[_index], this->effortLimit[_index]);
+
   ODEJoint::SetForce(_index, _force);
   if (this->childLink) this->childLink->SetEnabled(true);
   if (this->parentLink) this->parentLink->SetEnabled(true);
 
-  if (this->effortLimit[_index] > 0.0)
-    _force = math::clamp(_force,
-      -this->effortLimit[_index], this->effortLimit[_index]);
-
   // dJointAddScrewForce(this->jointId, _force);
   dJointAddScrewTorque(this->jointId, _force);
 }

gazebo/physics/ode/ODESliderJoint.cc

 //////////////////////////////////////////////////
 void ODESliderJoint::SetForce(int _index, double _force)
 {
+  if (_index < 0 || _index >= this->GetAngleCount())
+  {
+    gzerr << "Calling ODEScrewJoint::SetForce with an index ["
+          << _index << "] out of range\n";
+    return;
+  }
+
+  // truncate effort if effortLimit is not negative
+  if (this->effortLimit[_index] >= 0.0)
+    _force = math::clamp(_force, -this->effortLimit[_index],
+      this->effortLimit[_index]);
+
+
   ODEJoint::SetForce(_index, _force);
   if (this->childLink)
     this->childLink->SetEnabled(true);
   if (this->parentLink)
     this->parentLink->SetEnabled(true);
 
-  if (this->effortLimit[_index] > 0.0)
-    _force = math::clamp(_force, -this->effortLimit[_index],
-      this->effortLimit[_index]);
-
   dJointAddSliderForce(this->jointId, _force);
 }
 

gazebo/physics/ode/ODEUniversalJoint.cc

 //////////////////////////////////////////////////
 void ODEUniversalJoint::SetForce(int _index, double _torque)
 {
+  if (_index < 0 || _index >= this->GetAngleCount())
+  {
+    gzerr << "Calling ODEUniversalJoint::SetForce with an index ["
+          << _index << "] out of range\n";
+    return;
+  }
+
+  // truncate effort if effortLimit is not negative
+  if (this->effortLimit[_index] >= 0.0)
+    _torque = math::clamp(_torque, -this->effortLimit[_index],
+      this->effortLimit[_index]);
+
   ODEJoint::SetForce(_index, _torque);
   if (this->childLink) this->childLink->SetEnabled(true);
   if (this->parentLink) this->parentLink->SetEnabled(true);
 
-  if (this->effortLimit[_index] > 0.0)
-    _torque = math::clamp(_torque, -this->effortLimit[_index],
-      this->effortLimit[_index]);
-
   if (_index == 0)
     dJointAddUniversalTorques(this->jointId, _torque, 0);
   else