|
From: <rob...@us...> - 2008-08-18 18:05:39
|
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.
|