joint_calibration_simulator.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 // Author: Wim Meeussen
00031 
00032 #include "pr2_mechanism_model/joint_calibration_simulator.h"
00033 #include <angles/angles.h>
00034 
00035 
00036 namespace pr2_mechanism_model {
00037 
00038 
00039 JointCalibrationSimulator::JointCalibrationSimulator()
00040   : calibration_initialized_(false),
00041     calibration_has_rising_(false), 
00042     calibration_has_falling_(false),
00043     calibration_continuous_(false),
00044     got_info_(false)
00045 {
00046 }
00047 
00048 void JointCalibrationSimulator::GetJointCalibrationInfo(pr2_mechanism_model::JointState* js)
00049 {
00050   assert(js->joint_);
00051 
00052   // simulate calibration backward propagation
00053   if (js->joint_->calibration){
00054     if (js->joint_->calibration->rising){
00055       calibration_has_rising_ = true;
00056       calibration_rising_ = *(js->joint_->calibration->rising);
00057     }
00058     if (js->joint_->calibration->falling){
00059       calibration_has_falling_ = true;
00060       calibration_falling_ = *(js->joint_->calibration->falling);
00061     }
00062   }
00063   
00064 
00065   // continuous joints
00066   if (js->joint_->type == urdf::Joint::CONTINUOUS){
00067     calibration_continuous_ = true;
00068     if (calibration_has_rising_ && !calibration_has_falling_){
00069       calibration_has_falling_ = true;
00070       calibration_falling_ = calibration_rising_ + M_PI;
00071     }
00072     if (!calibration_has_rising_ && calibration_has_falling_){
00073       calibration_has_rising_ = true;
00074       calibration_rising_ = calibration_falling_ + M_PI;
00075     }
00076     calibration_rising_ = angles::normalize_angle(calibration_rising_);
00077     calibration_falling_ = angles::normalize_angle(calibration_falling_);
00078     if (calibration_rising_ < calibration_falling_)
00079       calibration_bump_ = true;
00080     else
00081       calibration_bump_ = false;
00082   }
00083 
00084   // check
00085   if (js->joint_->type != urdf::Joint::CONTINUOUS && calibration_has_rising_ && calibration_has_falling_)
00086       ROS_ERROR("Non-continuous joint with both rising and falling edge not supported");
00087 
00088   this->got_info_ = true;
00089 }
00090 
00091 void JointCalibrationSimulator::simulateJointCalibration(pr2_mechanism_model::JointState* js, pr2_hardware_interface::Actuator* as)
00092 {
00093   // setup calibration information for the joint
00094   if (!this->got_info_) this->GetJointCalibrationInfo(js);
00095 
00096   // current joint_angle
00097   double pos = js->position_ - js->reference_position_;
00098   double as_pos = as->state_.position_;
00099 
00100   // compute calibration reading
00101   if (calibration_continuous_){
00102     double pos_c = angles::normalize_angle(pos);
00103     if (calibration_bump_){
00104       if (pos_c > calibration_rising_ && pos_c < calibration_falling_)
00105         as->state_.calibration_reading_ = true;
00106       else
00107         as->state_.calibration_reading_ = false;
00108     }
00109     else{
00110       if (pos_c < calibration_rising_ && pos_c > calibration_falling_)
00111         as->state_.calibration_reading_ = false; // in low part
00112       else
00113         as->state_.calibration_reading_ = true;
00114     }
00115   }
00116   else{
00117     if (calibration_has_rising_)
00118       as->state_.calibration_reading_ = pos > calibration_rising_;
00119     else if (calibration_has_falling_)
00120       as->state_.calibration_reading_ = pos < calibration_falling_;
00121   }
00122 
00123   if (calibration_initialized_){
00124     // tripped calibration flag
00125     //ROS_ERROR("debug: %s %d %d",js->joint_->name.c_str(),old_calibration_reading_ ,as->state_.calibration_reading_);
00126     if (old_calibration_reading_ != as->state_.calibration_reading_){
00127       if (as->state_.calibration_reading_){ // low to high
00128         if (pos > old_calibration_pos_) // joint pos increasing and we are in the high region
00129         {
00130           as->state_.calibration_rising_edge_valid_ = true;
00131           as->state_.last_calibration_rising_edge_ = old_calibration_as_pos_;
00132         }
00133         else
00134         {
00135           as->state_.calibration_falling_edge_valid_ = true;
00136           as->state_.last_calibration_falling_edge_ = old_calibration_as_pos_;
00137         }
00138       }
00139       else{ // high to low
00140         if (pos > old_calibration_pos_) // joint pos increasing and we are in the low region
00141         {
00142           as->state_.calibration_falling_edge_valid_ = true;
00143           as->state_.last_calibration_falling_edge_ = old_calibration_as_pos_;
00144         }
00145         else
00146         {
00147           as->state_.calibration_rising_edge_valid_ = true;
00148           as->state_.last_calibration_rising_edge_ = old_calibration_as_pos_;
00149         }
00150       }
00151     }
00152   }
00153 
00154   // store state
00155   old_calibration_reading_ = as->state_.calibration_reading_;
00156   old_calibration_pos_ = pos;
00157   old_calibration_as_pos_ = as_pos;
00158   calibration_initialized_ = true;
00159 }
00160 
00161 
00162 
00163 } //namespace
00164 


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