[Personalrobots-commit] SF.net SVN: personalrobots:[3202] pkg/trunk

[<rob...@us...>]

Revision: 3202
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3202&view=rev
Author:   rob_wheeler
Date:     2008-08-18 18:05:38 +0000 (Mon, 18 Aug 2008)

Log Message:
-----------
Enforce joint effort limits in the joint, not in the controllers.

Modified Paths:
--------------
    pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_autotuner.h
    pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_effort_controller.h
    pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_position_controller.h
    pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_velocity_controller.h
    pkg/trunk/controllers/generic_controllers/include/generic_controllers/ramp_input_controller.h
    pkg/trunk/controllers/generic_controllers/src/joint_autotuner.cpp
    pkg/trunk/controllers/generic_controllers/src/joint_effort_controller.cpp
    pkg/trunk/controllers/generic_controllers/src/joint_position_controller.cpp
    pkg/trunk/controllers/generic_controllers/src/joint_velocity_controller.cpp
    pkg/trunk/controllers/generic_controllers/src/ramp_input_controller.cpp
    pkg/trunk/mechanism/mechanism_model/src/joint.cpp

Modified: pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_autotuner.h
===================================================================
--- pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_autotuner.h	2008-08-18 17:41:09 UTC (rev 3201)
+++ pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_autotuner.h	2008-08-18 18:05:38 UTC (rev 3202)
@@ -139,11 +139,6 @@
   double cycle_start_time_;
 
 private:
-  /*!
-   * \brief Actually issue torque set command of the joint motor.
-   */
-  void setJointEffort(double torque);
-
   mechanism::Joint* joint_;  /**< Joint we're controlling. */
   Pid pid_controller_;       /**< Internal PID controller. */
   double last_time_;         /**< Last time stamp of update. */

Modified: pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_effort_controller.h
===================================================================
--- pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_effort_controller.h	2008-08-18 17:41:09 UTC (rev 3201)
+++ pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_effort_controller.h	2008-08-18 18:05:38 UTC (rev 3202)
@@ -105,11 +105,6 @@
   virtual void update();
 
 private:
-  /*!
-   * \brief Actually issue torque set command of the joint motor.
-   */
-  void setJointEffort(double torque);
-
   mechanism::Joint* joint_; /**< Joint we're controlling. */
   mechanism::Robot *robot_; /**< Pointer to robot structure. */
   double command_;          /**< Last commanded position. */

Modified: pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_position_controller.h
===================================================================
--- pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_position_controller.h	2008-08-18 17:41:09 UTC (rev 3201)
+++ pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_position_controller.h	2008-08-18 18:05:38 UTC (rev 3202)
@@ -112,11 +112,6 @@
   virtual void update();
 
 private:
-  /*!
-   * \brief Actually issue torque set command of the joint motor.
-   */
-  void setJointEffort(double torque);
-
   mechanism::Joint* joint_;  /**< Joint we're controlling. */
   mechanism::Robot *robot_;  /**< Pointer to robot structure. */
   Pid pid_controller_;       /**< Internal PID controller. */

Modified: pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_velocity_controller.h
===================================================================
--- pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_velocity_controller.h	2008-08-18 17:41:09 UTC (rev 3201)
+++ pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_velocity_controller.h	2008-08-18 18:05:38 UTC (rev 3202)
@@ -112,11 +112,6 @@
   virtual void update();
 
 private:
-  /*!
-   * \brief Actually issue torque set command of the joint motor.
-   */
-  void setJointEffort(double torque);
-
   mechanism::Joint* joint_; /**< Joint we're controlling. */
   mechanism::Robot *robot_; /**< Pointer to robot structure. */
   Pid pid_controller_;      /**< Internal PID controller. */

Modified: pkg/trunk/controllers/generic_controllers/include/generic_controllers/ramp_input_controller.h
===================================================================
--- pkg/trunk/controllers/generic_controllers/include/generic_controllers/ramp_input_controller.h	2008-08-18 17:41:09 UTC (rev 3201)
+++ pkg/trunk/controllers/generic_controllers/include/generic_controllers/ramp_input_controller.h	2008-08-18 18:05:38 UTC (rev 3202)
@@ -108,11 +108,6 @@
   virtual void update();
 
 private:
-  /*!
-   * \brief Actually issue torque set command of the joint motor.
-   */
-  void setJointEffort(double torque);
-
   mechanism::Joint* joint_;     /**< Joint we're controlling. */
   mechanism::Robot *robot_;     /**< Pointer to robot structure. */
   double input_start_;          /**< Begining of the ramp. */

Modified: pkg/trunk/controllers/generic_controllers/src/joint_autotuner.cpp
===================================================================
--- pkg/trunk/controllers/generic_controllers/src/joint_autotuner.cpp	2008-08-18 17:41:09 UTC (rev 3201)
+++ pkg/trunk/controllers/generic_controllers/src/joint_autotuner.cpp	2008-08-18 18:05:38 UTC (rev 3202)
@@ -31,7 +31,6 @@
  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  *  POSSIBILITY OF SUCH DAMAGE.
  *********************************************************************/
-#include <algorithm>
 
 #include <generic_controllers/joint_autotuner.h>
 #include <math_utils/angles.h>
@@ -149,23 +148,18 @@
   if(current_state_ == POSITIVE_PEAK)
   {
     if(position > positive_peak_) positive_peak_ = position;
-  //If looking for positive peak, set positive h
-    setJointEffort(relay_height_);
+    //If looking for positive peak, set positive h
+    joint_->commanded_effort_ = relay_height_;
   }
   else if(current_state_ == NEGATIVE_PEAK)
   {    
     if(position<negative_peak_) negative_peak_ = position;
-  //If looking for negative peak, set negative h
-    setJointEffort(-relay_height_);
+    //If looking for negative peak, set negative h
+    joint_->commanded_effort_ = -relay_height_;
   }
 
 }
 
-void JointAutotuner::setJointEffort(double effort)
-{
-  joint_->commanded_effort_ = min(max(effort, -joint_->effort_limit_), joint_->effort_limit_);
-}
-
 ROS_REGISTER_CONTROLLER(JointAutotunerNode)
 JointAutotunerNode::JointAutotunerNode() 
 {

Modified: pkg/trunk/controllers/generic_controllers/src/joint_effort_controller.cpp
===================================================================
--- pkg/trunk/controllers/generic_controllers/src/joint_effort_controller.cpp	2008-08-18 17:41:09 UTC (rev 3201)
+++ pkg/trunk/controllers/generic_controllers/src/joint_effort_controller.cpp	2008-08-18 18:05:38 UTC (rev 3202)
@@ -31,7 +31,6 @@
  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  *  POSSIBILITY OF SUCH DAMAGE.
  *********************************************************************/
-#include <algorithm>
 #include <generic_controllers/joint_effort_controller.h>
 
 using namespace std;
@@ -95,14 +94,9 @@
 void JointEffortController::update()
 {
 
-  setJointEffort(command_);
+  joint_->commanded_effort_ = command_;
 }
 
-void JointEffortController::setJointEffort(double effort)
-{
-  joint_->commanded_effort_ = min(max(effort, -joint_->effort_limit_), joint_->effort_limit_);
-}
-
 ROS_REGISTER_CONTROLLER(JointEffortControllerNode)
 JointEffortControllerNode::JointEffortControllerNode() 
 {

Modified: pkg/trunk/controllers/generic_controllers/src/joint_position_controller.cpp
===================================================================
--- pkg/trunk/controllers/generic_controllers/src/joint_position_controller.cpp	2008-08-18 17:41:09 UTC (rev 3201)
+++ pkg/trunk/controllers/generic_controllers/src/joint_position_controller.cpp	2008-08-18 18:05:38 UTC (rev 3202)
@@ -31,7 +31,6 @@
  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  *  POSSIBILITY OF SUCH DAMAGE.
  *********************************************************************/
-#include <algorithm>
 #include <generic_controllers/joint_position_controller.h>
 #include <math_utils/angles.h>
 
@@ -106,7 +105,6 @@
 {
   assert(robot_ != NULL);
   double error(0);
-  double effort_cmd(0);
   double time = robot_->hw_->current_time_;
 
   if(joint_->type_ == mechanism::JOINT_ROTARY || joint_->type_ == mechanism::JOINT_CONTINUOUS)
@@ -118,16 +116,9 @@
     error = joint_->position_ - command_;
   }
 
-  effort_cmd = pid_controller_.updatePid(error, time - last_time_);
-
-  setJointEffort(effort_cmd);
+  joint_->commanded_effort_ = pid_controller_.updatePid(error, time - last_time_);
 }
 
-void JointPositionController::setJointEffort(double effort)
-{
-  joint_->commanded_effort_ = min(max(effort, -joint_->effort_limit_), joint_->effort_limit_);
-}
-
 ROS_REGISTER_CONTROLLER(JointPositionControllerNode)
 JointPositionControllerNode::JointPositionControllerNode()
 {

Modified: pkg/trunk/controllers/generic_controllers/src/joint_velocity_controller.cpp
===================================================================
--- pkg/trunk/controllers/generic_controllers/src/joint_velocity_controller.cpp	2008-08-18 17:41:09 UTC (rev 3201)
+++ pkg/trunk/controllers/generic_controllers/src/joint_velocity_controller.cpp	2008-08-18 18:05:38 UTC (rev 3202)
@@ -31,7 +31,6 @@
  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  *  POSSIBILITY OF SUCH DAMAGE.
  *********************************************************************/
-#include <algorithm>
 
 #include <generic_controllers/joint_velocity_controller.h>
 #include <math_utils/angles.h>
@@ -107,20 +106,12 @@
 void JointVelocityController::update()
 {
   double error(0);
-  double torque_cmd(0);
   double time = robot_->hw_->current_time_;
 
   error = joint_->velocity_ - command_;
-  torque_cmd = pid_controller_.updatePid(error, time - last_time_);
-
-  setJointEffort(torque_cmd);
+  joint_->commanded_effort_ = pid_controller_.updatePid(error, time - last_time_);
 }
 
-void JointVelocityController::setJointEffort(double effort)
-{
-  joint_->commanded_effort_ = min(max(effort, -joint_->effort_limit_), joint_->effort_limit_);
-}
-
 ROS_REGISTER_CONTROLLER(JointVelocityControllerNode)
 JointVelocityControllerNode::JointVelocityControllerNode() 
 {

Modified: pkg/trunk/controllers/generic_controllers/src/ramp_input_controller.cpp
===================================================================
--- pkg/trunk/controllers/generic_controllers/src/ramp_input_controller.cpp	2008-08-18 17:41:09 UTC (rev 3201)
+++ pkg/trunk/controllers/generic_controllers/src/ramp_input_controller.cpp	2008-08-18 18:05:38 UTC (rev 3202)
@@ -31,7 +31,6 @@
  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  *  POSSIBILITY OF SUCH DAMAGE.
  *********************************************************************/
-#include <algorithm>
 #include <generic_controllers/ramp_input_controller.h>
 
 using namespace std;
@@ -104,19 +103,11 @@
 
 void RampInputController::update()
 {
-  double effort_cmd(0);
   double time = robot_->hw_->current_time_;
   
-  effort_cmd=input_start_+(input_end_-input_start_)*(time-initial_time_)/(duration_);
-  
-  setJointEffort(effort_cmd);
+  joint_->commanded_effort_ = input_start_+(input_end_-input_start_)*(time-initial_time_)/(duration_);
 }
 
-void RampInputController::setJointEffort(double effort)
-{
-  joint_->commanded_effort_ = min(max(effort, -joint_->effort_limit_), joint_->effort_limit_);
-}
-
 ROS_REGISTER_CONTROLLER(RampInputControllerNode)
 RampInputControllerNode::RampInputControllerNode() 
 {

Modified: pkg/trunk/mechanism/mechanism_model/src/joint.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/joint.cpp	2008-08-18 17:41:09 UTC (rev 3201)
+++ pkg/trunk/mechanism/mechanism_model/src/joint.cpp	2008-08-18 18:05:38 UTC (rev 3202)
@@ -51,7 +51,7 @@
 
 void Joint::enforceLimits()
 {
-  // TODO: enforce the limits so the joint operates safely
+  commanded_effort_ = min(max(commanded_effort_, -effort_limit_), effort_limit_);
 }
 
 void Joint::initXml(TiXmlElement *elt)


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.