$search
00001 /* 00002 * UOS-ROS packages - Robot Operating System code by the University of Osnabrück 00003 * Copyright (C) 2011 University of Osnabrück 00004 * 00005 * This program is free software; you can redistribute it and/or 00006 * modify it under the terms of the GNU General Public License 00007 * as published by the Free Software Foundation; either version 2 00008 * of the License, or (at your option) any later version. 00009 * 00010 * This program is distributed in the hope that it will be useful, 00011 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00012 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00013 * GNU General Public License for more details. 00014 * 00015 * You should have received a copy of the GNU General Public License 00016 * along with this program; if not, write to the Free Software 00017 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 00018 * 00019 * Katana300.cpp 00020 * 00021 * Created on: Dec 13, 2011 00022 * Authors: 00023 * Hannes Raudies <h.raudies@hs-mannheim.de> 00024 * Martin Günther <mguenthe@uos.de> 00025 */ 00026 00027 #include <katana/Katana300.h> 00028 00029 namespace katana 00030 { 00031 00032 Katana300::Katana300() : 00033 Katana() 00034 { 00035 desired_angles_ = getMotorAngles(); 00036 setLimits(); 00037 } 00038 00039 Katana300::~Katana300() 00040 { 00041 } 00042 00043 void Katana300::setLimits() 00044 { 00045 // TODO: constants 00046 00047 // TODO: setting the limits this low shouldn't be necessary; the limits should 00048 // be set to 2 (acc.) and 180 (vel.) and tested on real Katana 300 00049 00050 00051 kni->setMotorAccelerationLimit(0, 1); 00052 kni->setMotorVelocityLimit(0, 30); 00053 00054 for (size_t i = 1; i < NUM_MOTORS; i++) 00055 { 00056 // These two settings probably only influence KNI functions like moveRobotToEnc(), 00057 // openGripper() and so on, and not the spline trajectories. We still set them 00058 // just to be sure. 00059 kni->setMotorAccelerationLimit(i, 1); 00060 kni->setMotorVelocityLimit(i, 25); 00061 } 00062 00063 } 00064 00069 void Katana300::freezeRobot() 00070 { 00071 boost::recursive_mutex::scoped_lock lock(kni_mutex); 00072 kni->freezeRobot(); 00073 } 00074 00078 bool Katana300::moveJoint(int jointIndex, double turningAngle) 00079 { 00080 00081 desired_angles_[jointIndex] = turningAngle; 00082 00083 return Katana::moveJoint(jointIndex, turningAngle); 00084 } 00085 00090 void Katana300::refreshMotorStatus() 00091 { 00092 Katana::refreshEncoders(); 00093 Katana::refreshMotorStatus(); 00094 } 00095 00100 bool Katana300::allJointsReady() 00101 { 00102 std::vector<double> motor_angles = getMotorAngles(); 00103 00104 for (size_t i = 0; i < NUM_JOINTS; i++) 00105 { 00106 if (motor_status_[i] == MSF_MOTCRASHED) 00107 return false; 00108 if (fabs(desired_angles_[i] - motor_angles[i]) > JOINTS_STOPPED_POS_TOLERANCE) 00109 return false; 00110 if (fabs(motor_velocities_[i]) > JOINTS_STOPPED_VEL_TOLERANCE) 00111 return false; 00112 } 00113 00114 return true; 00115 } 00116 00121 bool Katana300::allMotorsReady() 00122 { 00123 std::vector<double> motor_angles = getMotorAngles(); 00124 00125 for (size_t i = 0; i < NUM_MOTORS; i++) 00126 { 00127 if (motor_status_[i] == MSF_MOTCRASHED) 00128 return false; 00129 if (fabs(desired_angles_[i] - motor_angles[i]) > JOINTS_STOPPED_POS_TOLERANCE) 00130 return false; 00131 if (fabs(motor_velocities_[i]) > JOINTS_STOPPED_VEL_TOLERANCE) 00132 return false; 00133 } 00134 00135 return true; 00136 } 00137 00138 void Katana300::testSpeed() 00139 { 00140 ros::Rate idleWait(5); 00141 std::vector<double> pos1_angles(NUM_MOTORS); 00142 std::vector<double> pos2_angles(NUM_MOTORS); 00143 00144 // these are safe values, i.e., no self-collision is possible 00145 pos1_angles[0] = 2.88; 00146 pos2_angles[0] = -3.02; 00147 00148 pos1_angles[1] = 0.15; 00149 pos2_angles[1] = 2.16; 00150 00151 pos1_angles[2] = 1.40; 00152 pos2_angles[2] = -2.21; 00153 00154 pos1_angles[3] = 0.50; 00155 pos2_angles[3] = -2.02; 00156 00157 pos1_angles[4] = 2.86; 00158 pos2_angles[4] = -2.98; 00159 00160 pos1_angles[5] = -0.44; 00161 pos2_angles[5] = 0.30; 00162 00163 for (size_t i = 0; i < NUM_MOTORS; i++) 00164 { 00165 int pos1_encoders = (int)converter->angle_rad2enc(i, pos1_angles[i]); 00166 int pos2_encoders = (int)converter->angle_rad2enc(i, pos2_angles[i]); 00167 00168 int accel = kni->getMotorAccelerationLimit(i); 00169 int max_vel = kni->getMotorVelocityLimit(i); 00170 00171 ROS_INFO("Motor %zu - acceleration: %d (= %f), max speed: %d (=%f)", i, accel, 2.0 * converter->acc_enc2rad(i, accel), max_vel, converter->vel_enc2rad(i, max_vel)); 00172 ROS_INFO("KNI encoders: %d, %d", kni->GetBase()->GetMOT()->arr[i].GetEncoderMinPos(), kni->GetBase()->GetMOT()->arr[i].GetEncoderMaxPos()); 00173 ROS_INFO("moving to encoders: %d, %d", pos1_encoders, pos2_encoders); 00174 ROS_INFO("current encoders: %d", kni->getMotorEncoders(i, true)); 00175 00176 ROS_INFO("Moving to min"); 00177 { 00178 boost::recursive_mutex::scoped_lock lock(kni_mutex); 00179 kni->moveMotorToEnc(i, pos1_encoders, true, 50, 60000); 00180 } 00181 00182 // do 00183 // { 00184 // idleWait.sleep(); 00185 // refreshMotorStatus(); 00186 // 00187 // } while (!allMotorsReady()); 00188 00189 ROS_INFO("Moving to max"); 00190 { 00191 boost::recursive_mutex::scoped_lock lock(kni_mutex); 00192 kni->moveMotorToEnc(i, pos2_encoders, true, 50, 60000); 00193 } 00194 00195 // do 00196 // { 00197 // idleWait.sleep(); 00198 // refreshMotorStatus(); 00199 // } while (!allMotorsReady()); 00200 } 00201 00202 // Result: 00203 // Motor 0 - acceleration: 2 (= -4.908739), max speed: 180 (=-2.208932) 00204 // Motor 1 - acceleration: 2 (= -2.646220), max speed: 180 (=-1.190799) 00205 // Motor 2 - acceleration: 2 (= 5.292440), max speed: 180 (=2.381598) 00206 // --> wrong! the measured values are more like 2.6, 1.2 00207 // 00208 // Motor 3 - acceleration: 2 (= -4.908739), max speed: 180 (=-2.208932) 00209 // Motor 4 - acceleration: 2 (= -4.908739), max speed: 180 (=-2.208932) 00210 // Motor 5 - acceleration: 2 (= 1.597410), max speed: 180 (=0.718834) 00211 // (TODO: the gripper duration can be calculated from this) 00212 } 00213 00214 00215 }