Katana300.cpp
Go to the documentation of this file.
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  *    Benjamin Reiner <reinerbe@hs-weingarten.de>
00026  */
00027 
00028 #include <katana/Katana300.h>
00029 
00030 namespace katana
00031 {
00032 
00033 Katana300::Katana300() :
00034     Katana()
00035 {
00036   desired_angles_ = getMotorAngles();
00037   setLimits();
00038 }
00039 
00040 Katana300::~Katana300()
00041 {
00042 }
00043 
00044 void Katana300::setLimits()
00045 {
00046   // TODO: constants
00047 
00048   // TODO: setting the limits this low shouldn't be necessary; the limits should
00049   //       be set to 2 (acc.) and 180 (vel.) and tested on real Katana 300
00050   //fast: acc. 2 and vel. 150 (tested on real Katana 300)
00051   //slow: acc. 1 and vel. 30
00052 
00053 
00054   kni->setMotorAccelerationLimit(0, 2);
00055   kni->setMotorVelocityLimit(0, 60);    // set to 90 to protect our old Katana
00056 
00057   for (size_t i = 1; i < NUM_MOTORS; i++)
00058   {
00059     // These two settings probably only influence KNI functions like moveRobotToEnc(),
00060     // openGripper() and so on, and not the spline trajectories. We still set them
00061     // just to be sure.
00062     kni->setMotorAccelerationLimit(i, 2);
00063     kni->setMotorVelocityLimit(i, 60);
00064   }
00065 
00066 }
00067 
00072 void Katana300::freezeRobot()
00073 {
00074   boost::recursive_mutex::scoped_lock lock(kni_mutex);
00075   kni->freezeRobot();
00076 }
00077 
00081 bool Katana300::moveJoint(int jointIndex, double turningAngle)
00082 {
00083 
00084   desired_angles_[jointIndex] = turningAngle;
00085 
00086   return Katana::moveJoint(jointIndex, turningAngle);
00087 }
00088 
00093 void Katana300::refreshMotorStatus()
00094 {
00095   Katana::refreshEncoders();
00096   Katana::refreshMotorStatus();
00097 }
00098 
00103 bool Katana300::allJointsReady()
00104 {
00105   std::vector<double> motor_angles = getMotorAngles();
00106 
00107   for (size_t i = 0; i < NUM_JOINTS; i++)
00108   {
00109     if (motor_status_[i] == MSF_MOTCRASHED)
00110       return false;
00111     if (fabs(desired_angles_[i] - motor_angles[i]) > JOINTS_STOPPED_POS_TOLERANCE)
00112       return false;
00113     if (fabs(motor_velocities_[i]) > JOINTS_STOPPED_VEL_TOLERANCE)
00114       return false;
00115   }
00116 
00117   return true;
00118 }
00119 
00124 bool Katana300::allMotorsReady()
00125 {
00126   std::vector<double> motor_angles = getMotorAngles();
00127 
00128   for (size_t i = 0; i < NUM_MOTORS; i++)
00129   {
00130     if (motor_status_[i] == MSF_MOTCRASHED)
00131       return false;
00132 
00133     if (fabs(motor_velocities_[i]) > JOINTS_STOPPED_VEL_TOLERANCE)
00134       return false;
00135   }
00136 
00137   return true;
00138 }
00139 
00140 void Katana300::testSpeed()
00141 {
00142   ros::Rate idleWait(5);
00143   std::vector<double> pos1_angles(NUM_MOTORS);
00144   std::vector<double> pos2_angles(NUM_MOTORS);
00145 
00146   // these are safe values, i.e., no self-collision is possible
00147   // values on robot Kate
00148   pos1_angles[0] = 2.75;
00149   pos2_angles[0] = -1.5;
00150 
00151   pos1_angles[1] = 0.5;
00152   pos2_angles[1] = 2.1;
00153 
00154   pos1_angles[2] = 1.40;
00155   pos2_angles[2] = 0.3;
00156 
00157   pos1_angles[3] = 0.50;
00158   pos2_angles[3] = -2.00;
00159 
00160   pos1_angles[4] = 2.8;
00161   pos2_angles[4] = -2.75;
00162 
00163   pos1_angles[5] = -0.44;
00164   pos2_angles[5] = 0.30;
00165 
00166   for (size_t i = 0; i < NUM_MOTORS; i++)
00167   {
00168     int pos1_encoders = (int)converter->angle_rad2enc(i, pos1_angles[i]);
00169     int pos2_encoders = (int)converter->angle_rad2enc(i, pos2_angles[i]);
00170 
00171     int accel = kni->getMotorAccelerationLimit(i);
00172     int max_vel = kni->getMotorVelocityLimit(i);
00173 
00174     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));
00175     ROS_INFO("KNI encoders: %d, %d", kni->GetBase()->GetMOT()->arr[i].GetEncoderMinPos(), kni->GetBase()->GetMOT()->arr[i].GetEncoderMaxPos());
00176     ROS_INFO("moving to encoders: %d, %d", pos1_encoders, pos2_encoders);
00177     ROS_INFO("current encoders: %d", kni->getMotorEncoders(i, true));
00178 
00179     ROS_INFO("Moving to min");
00180     {
00181       boost::recursive_mutex::scoped_lock lock(kni_mutex);
00182       kni->moveMotorToEnc(i, pos1_encoders, true, 50, 60000);
00183     }
00184 
00185 //      do
00186 //      {
00187 //        idleWait.sleep();
00188 //        refreshMotorStatus();
00189 //
00190 //      } while (!allMotorsReady());
00191 
00192     ROS_INFO("Moving to max");
00193     {
00194       boost::recursive_mutex::scoped_lock lock(kni_mutex);
00195       kni->moveMotorToEnc(i, pos2_encoders, true, 50, 60000);
00196     }
00197 
00198 //      do
00199 //      {
00200 //        idleWait.sleep();
00201 //        refreshMotorStatus();
00202 //      } while (!allMotorsReady());
00203   }
00204 
00205   // Result:
00206   //  Motor 0 - acceleration: 2 (= -4.908739), max speed: 180 (=-2.208932)
00207   //  Motor 1 - acceleration: 2 (= -2.646220), max speed: 180 (=-1.190799)
00208   //  Motor 2 - acceleration: 2 (= 5.292440), max speed: 180 (=2.381598)
00209   //     --> wrong! the measured values are more like 2.6, 1.2
00210   //
00211   //  Motor 3 - acceleration: 2 (= -4.908739), max speed: 180 (=-2.208932)
00212   //  Motor 4 - acceleration: 2 (= -4.908739), max speed: 180 (=-2.208932)
00213   //  Motor 5 - acceleration: 2 (= 1.597410), max speed: 180 (=0.718834)
00214   //     (TODO: the gripper duration can be calculated from this)
00215 }
00216 
00223 bool Katana300::executeTrajectory(boost::shared_ptr<SpecifiedTrajectory> traj,
00224                                   boost::function<bool()> isPreemptRequested)
00225 {
00226   ROS_DEBUG("Entered executeTrajectory. Spline size: %d, trajectory size: %d, number of motors: %d",
00227             (int )traj->at(0).splines.size(), (int )traj->size(), kni->getNumberOfMotors());
00228   try
00229   {
00230     // ------- wait until all motors idle
00231     ros::Rate idleWait(10);
00232     while (!allMotorsReady())
00233     {
00234       refreshMotorStatus();
00235       ROS_DEBUG("Motor status: %d, %d, %d, %d, %d, %d", motor_status_[0], motor_status_[1], motor_status_[2],
00236                 motor_status_[3], motor_status_[4], motor_status_[5]);
00237 
00238       // ------- check if motors are blocked
00239       // it is important to do this inside the allMotorsReady() loop, otherwise we
00240       // could get stuck in a deadlock if the motors crash while we wait for them to
00241       // become ready
00242       if (someMotorCrashed())
00243       {
00244         ROS_WARN("Motors are crashed before executing trajectory! Unblocking...");
00245 
00246         boost::recursive_mutex::scoped_lock lock(kni_mutex);
00247         kni->unBlock();
00248       }
00249 
00250       idleWait.sleep();
00251     }
00252 
00253     // ------- wait until start time
00254     ros::Time start_time = ros::Time(traj->at(0).start_time);
00255     double time_until_start = (start_time - ros::Time::now()).toSec();
00256 
00257     if (time_until_start < -0.01)
00258     {
00259       ROS_WARN("Trajectory started %f s too late! Scheduled: %f, started: %f", -time_until_start, start_time.toSec(),
00260                ros::Time::now().toSec());
00261     }
00262     else if (time_until_start > 0.0)
00263     {
00264       ROS_DEBUG("Sleeping %f seconds until scheduled start of trajectory", time_until_start);
00265       ros::Time::sleepUntil(start_time);
00266     }
00267 
00268     // ------- start trajectory
00269     boost::recursive_mutex::scoped_lock lock(kni_mutex);
00270 
00271     // fix start times: set the trajectory start time to now(); since traj is a shared pointer,
00272     // this fixes the current_trajectory_ in joint_trajectory_action_controller, which synchronizes
00273     // the "state" publishing to the actual start time (more or less)
00274     double delay = ros::Time::now().toSec() - traj->at(0).start_time;
00275     for (size_t i = 0; i < traj->size(); i++)
00276     {
00277       traj->at(i).start_time += delay;
00278     }
00279 
00280     // enable splines with gripper
00281     bool isPresent;
00282     int openEncoder, closeEncoder;
00283     kni->getGripperParameters(isPresent, openEncoder, closeEncoder);
00284     kni->setGripperParameters(false, openEncoder, closeEncoder);
00285 
00286     // iterate over all trajectory steps
00287     for (size_t step = 0; step < traj->size(); step++)
00288     {
00289       ROS_DEBUG("Executing step %d", (int )step);
00290 
00291       if (isPreemptRequested())
00292       {
00293         ROS_INFO("Preempt requested. Aborting the trajectory!");
00294         return true;
00295       }
00296 
00297       Segment seg = traj->at(step);
00298 
00299       // + 1 to be flexible enough to perform trajectories that include the gripper
00300       if (seg.splines.size() != joint_names_.size() && seg.splines.size() != (joint_names_.size() + 1))
00301       {
00302         ROS_ERROR("Wrong number of joints in specified trajectory (was: %zu, expected: %zu)!", seg.splines.size(),
00303                   joint_names_.size());
00304       }
00305 
00306       refreshMotorStatus();
00307       if (someMotorCrashed())
00308       {
00309         ROS_ERROR("A motor crashed! Aborting to not destroy anything.");
00310         return false;
00311       }
00312 
00313       if (!ros::ok())
00314       {
00315         ROS_INFO("Stop trajectory because ROS node is stopped.");
00316         return true;
00317       }
00318 
00319       // time in 10 ms units, seg.duration is in seconds
00320       short duration = static_cast<short>(seg.duration * 100);
00321       // copy joint values and calculate to encoder values
00322       for (size_t jointNo = 0; jointNo < seg.splines.size(); jointNo++)
00323       {
00324         short encoder = static_cast<short>(converter->angle_rad2enc(jointNo, seg.splines[jointNo].target_position));
00325         desired_angles_[jointNo] = seg.splines[jointNo].target_position;
00326         // the actual position
00327         short p1 = ::round(converter->angle_rad2enc(jointNo, seg.splines[jointNo].coef[0]));
00328         short p2 = ::round(64 * converter->vel_rad2enc(jointNo, seg.splines[jointNo].coef[1]));
00329         short p3 = ::round(1024 * converter->acc_rad2enc(jointNo, seg.splines[jointNo].coef[2]));
00330         short p4 = ::round(32768 * converter->jerk_rad2enc(jointNo, seg.splines[jointNo].coef[3]));
00331 
00332         kni->sendSplineToMotor(jointNo, encoder, duration, p1, p2, p3, p4);
00333       }
00334 
00335       lock.unlock();
00336       ros::spinOnce();
00337       ros::Time::sleepUntil(ros::Time(seg.start_time));
00338 
00339       while (seg.start_time > ros::Time::now().toSec())
00340       {
00341         if (isPreemptRequested())
00342         {
00343           ROS_INFO("Preempt requested. Aborting the trajectory!");
00344           lock.lock();
00345           kni->freezeRobot();
00346           return true;
00347         }
00348         ros::spinOnce();
00349         ros::Duration(0.001).sleep();
00350       }
00351       lock.lock();
00352       kni->startSplineMovement(false);
00353     }
00354 
00355     //kni->moveRobotToEnc(encoders, true);      // to ensure that the goal position is reached
00356     return true;
00357   }
00358   catch (const WrongCRCException &e)
00359   {
00360     ROS_ERROR(
00361         "WrongCRCException: Two threads tried to access the KNI at once. This means that the locking in the Katana node is broken. (exception in executeTrajectory(): %s)",
00362         e.message().c_str());
00363   }
00364   catch (const ReadNotCompleteException &e)
00365   {
00366     ROS_ERROR(
00367         "ReadNotCompleteException: Another program accessed the KNI. Please stop it and restart the Katana node. (exception in executeTrajectory(): %s)",
00368         e.message().c_str());
00369   }
00370   catch (const FirmwareException &e)
00371   {
00372     // TODO: find out what the real cause of this is when it happens again
00373     // the message returned by the Katana is:
00374     // FirmwareException : 'StopperThread: collision on axis: 1 (axis N)'
00375     ROS_ERROR(
00376         "FirmwareException: Motor collision? Perhaps we tried to send a trajectory that the arm couldn't follow. (exception in executeTrajectory(): %s)",
00377         e.message().c_str());
00378   }
00379   catch (const MotorTimeoutException &e)
00380   {
00381     ROS_ERROR("MotorTimeoutException (exception in executeTrajectory(): %s)", e.what());
00382   }
00383   catch (const Exception &e)
00384   {
00385     ROS_ERROR("Unhandled exception in executeTrajectory(): %s", e.message().c_str());
00386   }
00387   catch (...)
00388   {
00389     ROS_ERROR("Unhandled exception in executeTrajectory()");
00390   }
00391 
00392   return false;
00393 }
00394 
00395 }


katana
Author(s): Martin Günther
autogenerated on Mon Aug 14 2017 02:45:49