joint_handle.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  *  Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2014-2015, Fetch Robotics Inc.
00005  *  Copyright (c) 2013-2014, Unbounded Robotics Inc.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of Fetch Robotics nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 // Author: Michael Ferguson
00037 
00038 #ifndef FETCH_GAZEBO_JOINT_HANDLE_H
00039 #define FETCH_GAZEBO_JOINT_HANDLE_H
00040 
00041 #include <ros/ros.h>
00042 #include <angles/angles.h>
00043 #include <control_toolbox/pid.h>
00044 #include <gazebo/physics/physics.hh>
00045 #include <robot_controllers_interface/joint_handle.h>
00046 #include <robot_controllers_interface/controller_manager.h>
00047 
00048 namespace gazebo
00049 {
00050 
00051 class JointHandle : public robot_controllers::JointHandle
00052 {
00053   enum CommandState
00054   {
00055     MODE_DISABLED,
00056     MODE_CONTROL_EFFORT,
00057     MODE_CONTROL_VELOCITY,
00058     MODE_CONTROL_POSITION
00059   };
00060 
00061 public:
00062   JointHandle(physics::JointPtr& joint,
00063               const double velocity_limit,
00064               const double effort_limit,
00065               const bool continuous) :
00066     joint_(joint),
00067     actual_velocity_(0.0),
00068     mode_(MODE_DISABLED)
00069   {
00070     ros::NodeHandle nh("~");
00071 
00072     // Load controller parameters
00073     position_pid_.init(ros::NodeHandle(nh, getName() + "/position"));
00074     velocity_pid_.init(ros::NodeHandle(nh, getName() + "/velocity"));
00075 
00076     // Set effort limit and continuous state
00077     velocity_limit_ = velocity_limit;
00078     effort_limit_ = effort_limit;
00079     continuous_ = continuous;
00080   }
00081   virtual ~JointHandle()
00082   {
00083   }
00084 
00091   virtual void setPosition(double position, double velocity, double effort)
00092   {
00093     // ControllerManager clears these all each cycle, so just
00094     // set mode and accumulate outputs of each controller.
00095     desired_position_ += position;
00096     desired_velocity_ += velocity;
00097     desired_effort_ += effort;
00098     mode_ = MODE_CONTROL_POSITION;
00099   }
00100 
00106   virtual void setVelocity(double velocity, double effort)
00107   {
00108     desired_velocity_ += velocity;
00109     desired_effort_ += effort;
00110     if (mode_ != MODE_CONTROL_POSITION)
00111       mode_ = MODE_CONTROL_VELOCITY;
00112   }
00113 
00118   virtual void setEffort(double effort)
00119   {
00120     desired_effort_ += effort;
00121     if (mode_ != MODE_CONTROL_POSITION &&
00122         mode_ != MODE_CONTROL_VELOCITY)
00123       mode_ = MODE_CONTROL_EFFORT;
00124   }
00125 
00127   virtual double getPosition()
00128   {
00129     if (continuous_)
00130     {
00131       return angles::normalize_angle(joint_->GetAngle(0).Radian());
00132     }
00133     return joint_->GetAngle(0).Radian();
00134   }
00135 
00137   virtual double getVelocity()
00138   {
00139     return actual_velocity_;
00140   }
00141 
00143   virtual double getEffort()
00144   {
00145     return applied_effort_;
00146   }
00147 
00148   /* Is this joint continuous (has no position limits). */
00149   virtual bool isContinuous()
00150   {
00151     return continuous_;
00152   }
00153 
00155   virtual double getPositionMin()
00156   {
00157     return joint_->GetLowerLimit(0).Radian();
00158   }
00159 
00161   virtual double getPositionMax()
00162   {
00163     return joint_->GetUpperLimit(0).Radian();
00164   }
00165 
00167   virtual double getVelocityMax()
00168   {
00169     if (velocity_limit_ < 0.0)
00170       return joint_->GetVelocityLimit(0);
00171     else
00172       return velocity_limit_;
00173   }
00174 
00176   virtual double getEffortMax()
00177   {
00178     /*
00179      * gzsdf has a major flaw when using continuous joints. It appears gazebo
00180      * cannot handle continuous joints and so it sets the limits to +/-1e16.
00181      * This is fine, except it drops the limit effort and limit velocity.
00182      * Lack of limit effort causes the robot to implode to the origin if the
00183      * controllers are not tuned or experience a disturbance. This little hack
00184      * lets us limit the controller effort internally.
00185      */
00186     if (effort_limit_ < 0.0)
00187       return joint_->GetEffortLimit(0);
00188     else
00189       return effort_limit_;
00190   }
00191 
00193   virtual std::string getName()
00194   {
00195     return joint_->GetName();
00196   } 
00197 
00199   virtual void reset()
00200   {
00201     desired_position_ = 0.0;
00202     desired_velocity_ = 0.0;
00203     desired_effort_ = 0.0;
00204     mode_ = MODE_DISABLED;
00205   }
00206 
00208   void update(const ros::Time now, const ros::Duration dt)
00209   {
00210     actual_velocity_ += 0.1 * (joint_->GetVelocity(0) - actual_velocity_);
00211 
00212     double effort = 0.0;
00213     if (mode_ == MODE_CONTROL_POSITION)
00214     {
00215       double p_error = angles::shortest_angular_distance(getPosition(), desired_position_);
00216       double v = position_pid_.computeCommand(p_error, dt) + desired_velocity_;
00217       v = std::min(getVelocityMax(), std::max(-getVelocityMax(), v));
00218       double t = velocity_pid_.computeCommand(v - actual_velocity_, dt);
00219       effort = t + desired_effort_;
00220     }
00221     else if (mode_ == MODE_CONTROL_VELOCITY)
00222     {
00223       double t = velocity_pid_.computeCommand(desired_velocity_ - actual_velocity_, dt);
00224       effort = t + desired_effort_;
00225     }
00226     else if (mode_ == MODE_CONTROL_EFFORT)
00227     {
00228       effort = desired_effort_;
00229     }
00230 
00231     // Limit effort so robot doesn't implode
00232     double lim = getEffortMax();
00233     applied_effort_ = std::max(-lim, std::min(effort, lim));
00234 
00235     // Actually update
00236     joint_->SetForce(0, applied_effort_);
00237   }
00238 
00239 private:
00240   physics::JointPtr joint_;
00241 
00242   double desired_position_;
00243   double desired_velocity_;
00244   double desired_effort_;
00245   
00247   int mode_;
00248 
00249   control_toolbox::Pid position_pid_;
00250   control_toolbox::Pid velocity_pid_;
00251 
00253   double velocity_limit_;
00254 
00256   double effort_limit_;
00257 
00258   // Is this joint continuous?
00259   bool continuous_;
00260 
00262   double applied_effort_;
00263   double actual_velocity_;
00264 
00265   // You no copy...
00266   JointHandle(const JointHandle&);
00267   JointHandle& operator=(const JointHandle&);
00268 };
00269 
00270 typedef boost::shared_ptr<JointHandle> JointHandlePtr;
00271 
00272 }  // namespace gazebo
00273 
00274 #endif  // FETCH_GAZEBO_JOINT_HANDLE_H


fetch_gazebo
Author(s): Michael Ferguson
autogenerated on Wed Apr 3 2019 02:48:45