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