joint.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #include <pr2_mechanism_model/joint.h>
00036 #include <map>
00037 #include <string>
00038 #include <vector>
00039 #include <cfloat>
00040 
00041 using namespace std;
00042 using namespace pr2_mechanism_model;
00043 
00044 
00045 void JointStatistics::update(JointState* jnt)
00046 {
00047   if (initialized_){
00048     odometer_ += fabs(old_position_ - jnt->position_);
00049     if (jnt->joint_->safety && jnt->joint_->limits && (fabs(jnt->commanded_effort_) > fabs(jnt->measured_effort_)))
00050       violated_limits_ = true;
00051     min_position_ = fmin(jnt->position_, min_position_);
00052     max_position_ = fmax(jnt->position_, max_position_);
00053     max_abs_velocity_ = fmax(fabs(jnt->velocity_), max_abs_velocity_);
00054     max_abs_effort_ = fmax(fabs(jnt->measured_effort_), max_abs_effort_);
00055   }
00056   else{
00057     min_position_ = jnt->position_;
00058     max_position_ = jnt->position_;
00059     initialized_ = true;
00060   }
00061   old_position_ = jnt->position_;
00062 }
00063 
00064 
00065 void JointStatistics::reset()
00066 {
00067   double tmp = min_position_;
00068   min_position_ = max_position_;
00069   max_position_ = tmp;
00070   max_abs_velocity_ = 0.0;
00071   max_abs_effort_ = 0.0;
00072   violated_limits_ = false;
00073 }
00074 
00075 
00076 void JointState::enforceLimits()
00077 {
00078   double effort_high, effort_low;
00079 
00080   getLimits(effort_low, effort_high);
00081 
00082   // limit the commanded effort based on position, velocity and effort limits
00083   commanded_effort_ =
00084     min( max(commanded_effort_, effort_low), effort_high);
00085 }
00086 
00087 void JointState::getLimits(double &effort_low, double &effort_high)
00088 {
00089   // only enforce joints that specify joint limits and safety code
00090   if (!joint_->safety || !joint_->limits) {
00091     effort_low = -std::numeric_limits<double>::max();
00092     effort_high = std::numeric_limits<double>::max();
00093     return;
00094   }
00095 
00096   double vel_high = joint_->limits->velocity;
00097   double vel_low = -joint_->limits->velocity;
00098   effort_high = joint_->limits->effort;
00099   effort_low = -joint_->limits->effort;
00100 
00101   // enforce position bounds on rotary and prismatic joints that are calibrated
00102   if (calibrated_ && (joint_->type == urdf::Joint::REVOLUTE || joint_->type == urdf::Joint::PRISMATIC))
00103   {
00104     // Computes the velocity bounds based on the absolute limit and the
00105     // proximity to the joint limit.
00106     vel_high = max(-joint_->limits->velocity,
00107                    min(joint_->limits->velocity,
00108                        -joint_->safety->k_position * (position_ - joint_->safety->soft_upper_limit)));
00109     vel_low = min(joint_->limits->velocity,
00110                   max(-joint_->limits->velocity,
00111                       -joint_->safety->k_position * (position_ - joint_->safety->soft_lower_limit)));
00112   }
00113 
00114   // Computes the effort bounds based on the velocity and effort bounds.
00115   effort_high = max(-joint_->limits->effort,
00116                     min(joint_->limits->effort,
00117                         -joint_->safety->k_velocity * (velocity_ - vel_high)));
00118   effort_low = min(joint_->limits->effort,
00119                    max(-joint_->limits->effort,
00120                        -joint_->safety->k_velocity * (velocity_ - vel_low)));
00121 }
00122 
00123 


pr2_mechanism_model
Author(s): Eric Berger berger@willowgarage.com, Stuart Glaser, Wim Meeussen
autogenerated on Mon Dec 2 2013 13:13:02