Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[4456] pkg/trunk
|
From: <stu...@us...> - 2008-09-18 16:51:12
|
Revision: 4456
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4456&view=rev
Author: stuglaser
Date: 2008-09-18 23:51:22 +0000 (Thu, 18 Sep 2008)
Log Message:
-----------
Updated mechanism_model to take into account the new location of Pid.
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_controller.h
pkg/trunk/mechanism/mechanism_model/CMakeLists.txt
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/gripper_transmission.h
pkg/trunk/mechanism/mechanism_model/manifest.xml
pkg/trunk/mechanism/mechanism_model/src/gripper_transmission.cpp
Removed Paths:
-------------
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/pid.h
pkg/trunk/mechanism/mechanism_model/src/pid.cpp
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_controller.h 2008-09-18 23:50:44 UTC (rev 4455)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_controller.h 2008-09-18 23:51:22 UTC (rev 4456)
@@ -128,7 +128,7 @@
private:
mechanism::JointState *joint_state_; /**< Joint we're controlling. */
mechanism::RobotState *robot_; /**< Pointer to robot structure. */
- control_toolbox::control_toolbox::Pid pid_controller_; /**< Internal PID controller. */
+ control_toolbox::Pid pid_controller_; /**< Internal PID controller. */
double last_time_; /**< Last time stamp of update. */
double command_; /**< Last commanded position. */
Modified: pkg/trunk/mechanism/mechanism_model/CMakeLists.txt
===================================================================
--- pkg/trunk/mechanism/mechanism_model/CMakeLists.txt 2008-09-18 23:50:44 UTC (rev 4455)
+++ pkg/trunk/mechanism/mechanism_model/CMakeLists.txt 2008-09-18 23:51:22 UTC (rev 4456)
@@ -1,4 +1,9 @@
cmake_minimum_required(VERSION 2.6)
include(rosbuild)
rospack(mechanism_model)
-rospack_add_library(mechanism_model src/simple_transmission.cpp src/gripper_transmission.cpp src/joint.cpp src/robot.cpp src/link.cpp src/pid.cpp)
+rospack_add_library( mechanism_model
+ src/simple_transmission.cpp
+ src/gripper_transmission.cpp
+ src/joint.cpp
+ src/robot.cpp
+ src/link.cpp)
Modified: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/gripper_transmission.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/gripper_transmission.h 2008-09-18 23:50:44 UTC (rev 4455)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/gripper_transmission.h 2008-09-18 23:51:22 UTC (rev 4456)
@@ -50,7 +50,7 @@
#include "tinyxml/tinyxml.h"
#include "mechanism_model/transmission.h"
#include "mechanism_model/robot.h"
-#include "mechanism_model/pid.h"
+#include "control_toolbox/pid.h"
namespace mechanism {
@@ -68,7 +68,7 @@
void propagateEffortBackwards(std::vector<Actuator*>&, std::vector<JointState*>&);
std::vector<double> reductions_; // Mechanical reduction for each joint
- std::vector<Pid> pids_; // For keeping the joint angles aligned in Gazebo
+ std::vector<control_toolbox::Pid> pids_; // For keeping the joint angles aligned in Gazebo
double motor_torque_constant_;
double pulses_per_revolution_;
Deleted: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/pid.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/pid.h 2008-09-18 23:50:44 UTC (rev 4455)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/pid.h 2008-09-18 23:51:22 UTC (rev 4456)
@@ -1,184 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of the Willow Garage nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-#pragma once
-
-/***************************************************/
-/*! \class controller::Pid
- \brief A basic pid class.
-
- This class implements a generic structure that
- can be used to create a wide range of pid
- controllers. It can function independently or
- be subclassed to provide more specific controls
- based on a particular control loop.
-
- In particular, this class implements the standard
- pid equation:
-
- command = -p_term_ - i_term_ - d_term_
-
- where: <br>
- <UL TYPE="none">
- <LI> p_term_ = p_gain_ * p_error_
- <LI> i_term_ = i_gain_ * i_error_
- <LI> d_term_ = d_gain_ * d_error_
- <LI> i_error_ = i_error_ + p_error_ * dt
- <LI> d_error_ = d_error_ + (p_error_ - p_error_last_) / dt
- </UL>
-
- given:<br>
- <UL TYPE="none">
- <LI> p_error_ = p_state-p_target.
- </UL>
-
-*/
-/***************************************************/
-class TiXmlElement;
-
-class Pid
-{
-public:
-
- /*!
- * \brief Constructor, zeros out Pid values when created and
- * initialize Pid-gains and integral term limits:[iMax:iMin]-[I1:I2].
- *
- * \param P The proportional gain.
- * \param I The integral gain.
- * \param D The derivative gain.
- * \param I1 The integral upper limit.
- * \param I2 The integral lower limit.
- */
- Pid(double P = 0.0, double I = 0.0, double D = 0.0, double I1 = 0.0, double I2 = -0.0);
-
- /*!
- * \brief Destructor of Pid class.
- */
- ~Pid();
-
- /*!
- * \brief Update the Pid loop with nonuniform time step size.
- *
- * \param p_error Error since last call (p_state-p_target)
- * \param dt Change in time since last call
- */
- double updatePid(double p_error, double dt);
-
- /*!
- * \brief Initialize PID-gains and integral term limits:[iMax:iMin]-[I1:I2]
- *
- * \param P The proportional gain.
- * \param I The integral gain.
- * \param D The derivative gain.
- * \param I1 The integral upper limit.
- * \param I2 The integral lower limit.
- */
- void initPid(double P, double I, double D, double I1, double I2);
-
- void initXml(TiXmlElement *config);
-
- /*!
- * \brief Set current command for this PID controller
- */
- void setCurrentCmd(double cmd);
-
- /*!
- * \brief Return current command for this PID controller
- */
- double getCurrentCmd();
-
- /*!
- * \brief Return PID error terms for the controller.
- * \param pe The proportional error.
- * \param ie The integral error.
- * \param de The derivative error.
- */
- void getCurrentPIDErrors(double *pe, double *ie, double *de);
-
- /*!
- * \brief Set PID gains for the controller.
- * \param P The proportional gain.
- * \param I The integral gain.
- * \param D The derivative gain.
- * \param i_max
- * \param i_min
- */
- void setGains(double P, double I, double D, double i_max, double i_min);
-
- /*!
- * \brief Get PID gains for the controller.
- * \param p The proportional gain.
- * \param i The integral gain.
- * \param d The derivative gain.
- * \param i_max
- * \param i_mim
- */
- void getGains(double &p, double &i, double &d, double &i_max, double &i_min);
-
- /*!
- * \brief Update the Pid loop with nonuniform time step size.
- *
- * \param error Error since last call (p_state-p_target)
- * \param error_dot d(Error)/dt since last call
- * \param dt Change in time since last call
- */
- double updatePid(double error, double error_dot, double dt);
-
- Pid &operator =(const Pid& p)
- {
- if (this == &p)
- return *this;
-
- p_gain_ = p.p_gain_;
- i_gain_ = p.i_gain_;
- d_gain_ = p.d_gain_;
- i_max_ = p.i_max_;
- i_min_ = p.i_min_;
-
- p_error_last_ = p_error_ = i_error_ = d_error_ = cmd_ = 0.0;
- return *this;
- }
-
-private:
- double p_error_last_; /**< _Save position state for derivative state calculation. */
- double p_error_; /**< Position error. */
- double d_error_; /**< Derivative error. */
- double i_error_; /**< Integator error. */
- double p_gain_; /**< Proportional gain. */
- double i_gain_; /**< Integral gain. */
- double d_gain_; /**< Derivative gain. */
- double i_max_; /**< Maximum allowable integral term. */
- double i_min_; /**< Minimum allowable integral term. */
- double cmd_; /**< Command to send. */
-};
Modified: pkg/trunk/mechanism/mechanism_model/manifest.xml
===================================================================
--- pkg/trunk/mechanism/mechanism_model/manifest.xml 2008-09-18 23:50:44 UTC (rev 4455)
+++ pkg/trunk/mechanism/mechanism_model/manifest.xml 2008-09-18 23:51:22 UTC (rev 4456)
@@ -3,14 +3,15 @@
</description>
<author>Eric Berger be...@wi...</author>
<license>BSD</license>
-<depend package='roscpp'/>
-<depend package='hardware_interface'/>
+<depend package="roscpp" />
+<depend package="hardware_interface" />
<depend package="stl_utils" />
<depend package="misc_utils" />
<depend package="tinyxml" />
<depend package="wg_robot_description_parser" />
<depend package="kdl" />
<depend package="libTF" />
+<depend package="control_toolbox" />
<url>http://pr.willowgarage.com</url>
<export>
<cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lmechanism_model -Wl,-rpath,${prefix}/lib"/>
Modified: pkg/trunk/mechanism/mechanism_model/src/gripper_transmission.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/gripper_transmission.cpp 2008-09-18 23:50:44 UTC (rev 4455)
+++ pkg/trunk/mechanism/mechanism_model/src/gripper_transmission.cpp 2008-09-18 23:51:22 UTC (rev 4456)
@@ -76,7 +76,7 @@
TiXmlElement *pel = config->FirstChildElement("pid");
if (pel)
{
- Pid pid;
+ control_toolbox::Pid pid;
pid.initXml(pel);
for (unsigned int i = 0; i < pids_.size(); ++i)
pids_[i] = pid;
Deleted: pkg/trunk/mechanism/mechanism_model/src/pid.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/pid.cpp 2008-09-18 23:50:44 UTC (rev 4455)
+++ pkg/trunk/mechanism/mechanism_model/src/pid.cpp 2008-09-18 23:51:22 UTC (rev 4456)
@@ -1,191 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of the Willow Garage nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-#include "mechanism_model/pid.h"
-#include "tinyxml/tinyxml.h"
-
-Pid::Pid(double P, double I, double D, double I1, double I2) :
- p_gain_(P), i_gain_(I), d_gain_(D), i_max_(I1), i_min_(I2)
-{
- p_error_last_ = 0.0;
- p_error_ = 0.0;
- d_error_ = 0.0;
- i_error_ = 0.0;
- cmd_ = 0.0;
-}
-
-Pid::~Pid()
-{
-}
-
-void Pid::initPid(double P, double I, double D, double I1, double I2)
-{
- p_gain_ = P;
- i_gain_ = I;
- d_gain_ = D;
- i_max_ = I1;
- i_min_ = I2;
- p_error_last_ = 0.0;
- p_error_ = 0.0;
- d_error_ = 0.0;
- i_error_ = 0.0;
- cmd_ = 0.0;
-}
-
-void Pid::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
-{
- p = p_gain_;
- i = i_gain_;
- d = d_gain_;
- i_max = i_max_;
- i_min = i_min_;
-}
-
-void Pid::setGains(double P, double I, double D, double I1, double I2)
-{
- p_gain_ = P;
- i_gain_ = I;
- d_gain_ = D;
- i_max_ = I1;
- i_min_ = I2;
-}
-
-void Pid::initXml(TiXmlElement *config)
-{
- p_gain_ = config->Attribute("p") ? atof(config->Attribute("p")) : 0.0;
- i_gain_ = config->Attribute("i") ? atof(config->Attribute("i")) : 0.0;
- d_gain_ = config->Attribute("d") ? atof(config->Attribute("d")) : 0.0;
- i_max_ = config->Attribute("iClamp") ? atof(config->Attribute("iClamp")) : 0.0;
- i_min_ = -i_max_;
-}
-
-double Pid::updatePid(double error, double dt)
-{
- double p_term, d_term, i_term;
- p_error_ = error; //this is pError = pState-pTarget
-
- if (dt == 0)
- {
- throw "dividebyzero"; //TODO: not sure how to deal with this
- }
- else
- {
- // Calculate proportional contribution to command
- p_term = p_gain_ * p_error_;
-
- // Calculate the integral error
- i_error_ = i_error_ + dt * p_error_;
-
- //Calculate integral contribution to command
- i_term = i_gain_ * i_error_;
-
- // Limit i_term so that the limit is meaningful in the output
- if (i_term > i_max_)
- {
- i_term = i_max_;
- }
- else if (i_term < i_min_)
- {
- i_term = i_min_;
- }
-
- // Calculate the derivative error
- if (dt != 0)
- {
- d_error_ = (p_error_ - p_error_last_) / dt;
- p_error_last_ = p_error_;
- }
- // Calculate derivative contribution to command
- d_term = d_gain_ * d_error_;
- cmd_ = -p_term - i_term - d_term;
- }
- return cmd_;
-}
-
-
-double Pid::updatePid(double error, double error_dot, double dt)
-{
- double p_term, d_term, i_term;
- p_error_ = error; //this is pError = pState-pTarget
- d_error_ = error_dot;
- if (dt == 0)
- {
- throw "dividebyzero"; //TODO: not sure how to deal with this
- }
- else
- {
- // Calculate proportional contribution to command
- p_term = p_gain_ * p_error_;
-
- // Calculate the integral error
- i_error_ = i_error_ + dt * p_error_;
-
- //Calculate integral contribution to command
- i_term = i_gain_ * i_error_;
-
- // Limit i_term so that the limit is meaningful in the output
- if (i_term > i_max_)
- {
- i_term = i_max_;
- }
- else if (i_term < i_min_)
- {
- i_term = i_min_;
- }
-
- // Calculate derivative contribution to command
- d_term = d_gain_ * d_error_;
- cmd_ = -p_term - i_term - d_term;
- }
- return cmd_;
-}
-
-
-
-void Pid::setCurrentCmd(double cmd)
-{
- cmd_ = cmd;
-}
-
-double Pid::getCurrentCmd()
-{
- return cmd_;
-}
-
-void Pid::getCurrentPIDErrors(double *pe, double *ie, double *de)
-{
- *pe = p_error_;
- *ie = i_error_;
- *de = d_error_;
-}
-
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
