Katana.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) 2010  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  * Katana.cpp
00020  *
00021  *  Created on: 06.12.2010
00022  *      Author: Martin Günther <mguenthe@uos.de>
00023  *      Co-Author: Henning Deeken <hdeeken@uos.de>
00024  */
00025 
00026 #include "katana/Katana.h"
00027 
00028 namespace katana
00029 {
00030 
00031 Katana::Katana() :
00032   AbstractKatana()
00033 {
00034   ros::NodeHandle nh;
00035   ros::NodeHandle pn("~");
00036   motor_status_.resize(NUM_MOTORS);
00037 
00038   /* ********* get parameters ********* */
00039 
00040   // general parameters
00041   std::string config_file_path;
00042   bool use_serial;
00043 
00044   bool has_path = pn.getParam("config_file_path", config_file_path);
00045   if (!has_path) {
00046     ROS_ERROR("Required parameter config_file_path could not be found on param server!");
00047     return;
00048   }
00049 
00050   pn.param("use_serial", use_serial, false);
00051 
00052   converter = new KNIConverter(config_file_path);
00053 
00054   // parameters for TCP/IP connection
00055   std::string ip;
00056   int tcpip_port;
00057 
00058   pn.param<std::string> ("ip", ip, "192.168.1.1");
00059   pn.param("port", tcpip_port, 5566);
00060 
00061   // parameters for serial connection
00062   int serial_port;
00063 
00064   pn.param("serial_port", serial_port, 0);
00065   if (serial_port < 0 || serial_port > 9)
00066   {
00067     ROS_ERROR("serial_port must be in the range [0-9]!");
00068     return;
00069   }
00070 
00071   try
00072   {
00073     /* ********* open device ********* */
00074     if (use_serial)
00075     {
00076       ROS_INFO("trying to connect to katana (serial port: /dev/ttyS%d) ...", serial_port);
00077 
00078       TCdlCOMDesc serial_config;
00079       serial_config.port = serial_port; // serial port number (0-9 for /dev/ttyS[0-9])
00080       serial_config.baud = 57600; // baud rate of port
00081       serial_config.data = 8; // data bits
00082       serial_config.parity = 'N'; // parity bit
00083       serial_config.stop = 1; // stop bit
00084       serial_config.rttc = 300; // read  total timeout
00085       serial_config.wttc = 0; // write total timeout
00086 
00087       device = new CCdlCOM(serial_config);
00088       ROS_INFO("success: serial connection to Katana opened");
00089     }
00090     else
00091     {
00092       ROS_INFO("trying to connect to katana (TCP/IP) on %s:%d...", ip.c_str(), tcpip_port);
00093       char* nonconst_ip = strdup(ip.c_str());
00094       device = new CCdlSocket(nonconst_ip, tcpip_port);
00095       free(nonconst_ip);
00096       ROS_INFO("success: TCP/IP connection to Katana opened");
00097     }
00098 
00099     /* ********* init protocol ********* */
00100     protocol = new CCplSerialCRC();
00101     ROS_INFO("success: protocol class instantiated");
00102 
00103     protocol->init(device); //fails if no response from Katana
00104     ROS_INFO("success: communication with Katana initialized");
00105 
00106     /* ********* init robot ********* */
00107     kni.reset(new CLMBase());
00108     kni->create(config_file_path.c_str(), protocol);
00109     ROS_INFO("success: katana initialized");
00110   }
00111   catch (Exception &e)
00112   {
00113     ROS_ERROR("Exception during initialization: '%s'", e.message().c_str());
00114     return;
00115   }
00116 
00117   setLimits();
00118 
00119   /* ********* calibrate ********* */
00120   calibrate();
00121   ROS_INFO("success: katana calibrated");
00122 
00123   refreshEncoders();
00124 
00125   // boost::thread worker_thread(&Katana::test_speed, this);
00126 
00127   /* ********* services ********* */
00128   switch_motors_off_srv_ = nh.advertiseService("switch_motors_off", &Katana::switchMotorsOff, this);
00129   switch_motors_on_srv_ = nh.advertiseService("switch_motors_on", &Katana::switchMotorsOn, this);
00130   test_speed_srv_  = nh.advertiseService("test_speed", &Katana::testSpeedSrv, this);
00131 }
00132 
00133 Katana::~Katana()
00134 {
00135   // protocol and device are members of kni, so we should be
00136   // the last ones using it before deleting them
00137   assert(kni.use_count() == 1);
00138 
00139   kni.reset(); // delete kni, so protocol and device won't be used any more
00140   delete protocol;
00141   delete device;
00142   delete converter;
00143 }
00144 
00145 void Katana::setLimits(void) {
00146         for (size_t i = 0; i < NUM_MOTORS; i++) {
00147                 // These two settings probably only influence KNI functions like moveRobotToEnc(),
00148                 // openGripper() and so on, and not the spline trajectories. We still set them
00149                 // just to be sure.
00150                 kni->setMotorAccelerationLimit(i, KNI_MAX_ACCELERATION);
00151                 kni->setMotorVelocityLimit(i, KNI_MAX_VELOCITY);
00152         }
00153 }
00154 
00155 void Katana::refreshEncoders()
00156 {
00157   try
00158   {
00159     boost::recursive_mutex::scoped_lock lock(kni_mutex);
00160     CMotBase* motors = kni->GetBase()->GetMOT()->arr;
00161 
00162     kni->GetBase()->recvMPS(); // refresh all pvp->pos at once
00163 
00164     // Using recvMPS() instead of recvPVP() makes our updates 6 times
00165     // faster, since we only need one KNI call instead of 6. The KNI
00166     // needs exactly 40 ms for every update call.
00167 
00168     for (size_t i = 0; i < NUM_MOTORS; i++)
00169     {
00170       // motors[i].recvPVP(); // refresh pvp->pos, pvp->vel and pvp->msf for single motor; throws ParameterReadingException
00171       const TMotPVP* pvp = motors[i].GetPVP();
00172 
00173       double current_angle = converter->angle_enc2rad(i, pvp->pos);
00174       double time_since_update = (ros::Time::now() - last_encoder_update_).toSec();
00175 
00176       if (last_encoder_update_ == ros::Time(0.0) || time_since_update == 0.0)
00177       {
00178         motor_velocities_[i] = 0.0;
00179       }
00180       else
00181       {
00182         motor_velocities_[i] = (current_angle - motor_angles_[i]) / time_since_update;
00183       }
00184       //  // only necessary when using recvPVP():
00185       //  motor_velocities_[i] = vel_enc2rad(i, pvp->vel) * (-1);  // the -1 is because the KNI is inconsistent about velocities
00186 
00187       motor_angles_[i] = current_angle;
00188     }
00189 
00190     //  // This is an ugly workaround, but apparently the velocities returned by the
00191     //  // Katana are wrong by a factor of exactly 0.5 for motor 2 and a factor of -1
00192     //  // for motor 4. This is only necessary when using recvPVP() to receive the
00193     //  // velocities directly from the KNI.
00194     //  motor_velocities_[2] *= 0.5;
00195     //  motor_velocities_[4] *= -1.0;
00196 
00197     last_encoder_update_ = ros::Time::now();
00198   }
00199   catch (const WrongCRCException &e)
00200   {
00201     ROS_ERROR("WrongCRCException: Two threads tried to access the KNI at once. This means that the locking in the Katana node is broken. (exception in refreshEncoders(): %s)", e.message().c_str());
00202   }
00203   catch (const ReadNotCompleteException &e)
00204   {
00205     ROS_ERROR("ReadNotCompleteException: Another program accessed the KNI. Please stop it and restart the Katana node. (exception in refreshEncoders(): %s)", e.message().c_str());
00206   }
00207   catch (const ParameterReadingException &e)
00208   {
00209     ROS_ERROR("ParameterReadingException: Could not receive PVP (Position Velocity PWM) parameters from a motor (exception in refreshEncoders(): %s)", e.message().c_str());
00210   }
00211   catch (const FirmwareException &e)
00212   {
00213     // This can happen when the arm collides with something (red LED).
00214     // The message returned by the Katana in this case is:
00215     // FirmwareException : 'move buffer error (axis 1)'
00216     ROS_ERROR("FirmwareException: Did the arm collide with something (red LED)? (exception in refreshEncoders(): %s)", e.message().c_str());
00217   }
00218   catch (const Exception &e)
00219   {
00220     ROS_ERROR("Unhandled exception in refreshEncoders(): %s", e.message().c_str());
00221   }
00222   catch (...)
00223   {
00224     ROS_ERROR("Unhandled exception in refreshEncoders()");
00225   }
00226 }
00227 
00228 void Katana::refreshMotorStatus()
00229 {
00230   try
00231   {
00232     boost::recursive_mutex::scoped_lock lock(kni_mutex);
00233     CMotBase* motors = kni->GetBase()->GetMOT()->arr;
00234 
00235     kni->GetBase()->recvGMS(); // refresh all pvp->msf at once
00236 
00237     for (size_t i = 0; i < NUM_MOTORS; i++)
00238     {
00239       const TMotPVP* pvp = motors[i].GetPVP();
00240 
00241       motor_status_[i] = pvp->msf;
00242       //  MSF_MECHSTOP    = 1,    //!< mechanical stopper reached, new: unused (default value)
00243       //  MSF_MAXPOS      = 2,    //!< max. position was reached, new: unused
00244       //  MSF_MINPOS      = 4,    //!< min. position was reached, new: calibrating
00245       //  MSF_DESPOS      = 8,    //!< in desired position, new: fixed, state holding
00246       //  MSF_NORMOPSTAT  = 16,   //!< trying to follow target, new: moving (polymb not full)
00247       //  MSF_MOTCRASHED  = 40,   //!< motor has crashed, new: collision (MG: this means that the motor has reached a collision limit (e.g., after executing a invalid spline) and has to be reset using unBlock())
00248       //  MSF_NLINMOV     = 88,   //!< non-linear movement ended, new: poly move finished
00249       //  MSF_LINMOV      = 152,  //!< linear movement ended, new: moving poly, polymb full
00250       //  MSF_NOTVALID    = 128   //!< motor data not valid
00251 
00252       /*
00253        *  Das Handbuch zu Katana4D sagt (zu ReadAxisState):
00254        *    0 = off (ausgeschaltet)
00255        *    8 = in gewünschter Position (Roboter fixiert)
00256        *   24 = im "normalen Bewegungsstatus"; versucht die gewünschte Position zu erreichen
00257        *   40 = wegen einer Kollision gestoppt
00258        *   88 = die Linearbewegung ist beendet
00259        *  128 = ungültige Achsendaten (interne Kommunikationsprobleme)
00260        */
00261     }
00262   }
00263   catch (const WrongCRCException &e)
00264   {
00265     ROS_ERROR("WrongCRCException: Two threads tried to access the KNI at once. This means that the locking in the Katana node is broken. (exception in refreshMotorStatus(): %s)", e.message().c_str());
00266   }
00267   catch (const ReadNotCompleteException &e)
00268   {
00269     ROS_ERROR("ReadNotCompleteException: Another program accessed the KNI. Please stop it and restart the Katana node. (exception in refreshMotorStatus(): %s)", e.message().c_str());
00270   }
00271   catch (const Exception &e)
00272   {
00273     ROS_ERROR("Unhandled exception in refreshMotorStatus(): %s", e.message().c_str());
00274   }
00275   catch (...)
00276   {
00277     ROS_ERROR("Unhandled exception in refreshMotorStatus()");
00278   }
00279 }
00280 
00286 bool Katana::executeTrajectory(boost::shared_ptr<SpecifiedTrajectory> traj, boost::function<bool ()> isPreemptRequested)
00287 {
00288   assert(traj->size() > 0);
00289 
00290   try
00291   {
00292     // ------- wait until all motors idle
00293     ros::Rate idleWait(10);
00294     while (!allMotorsReady())
00295     {
00296       refreshMotorStatus();
00297       ROS_DEBUG("Motor status: %d, %d, %d, %d, %d, %d", motor_status_[0], motor_status_[1], motor_status_[2], motor_status_[3], motor_status_[4], motor_status_[5]);
00298 
00299       // ------- check if motors are blocked
00300       // it is important to do this inside the allMotorsReady() loop, otherwise we
00301       // could get stuck in a deadlock if the motors crash while we wait for them to
00302       // become ready
00303       if (someMotorCrashed())
00304       {
00305         ROS_WARN("Motors are crashed before executing trajectory! Unblocking...");
00306 
00307         boost::recursive_mutex::scoped_lock lock(kni_mutex);
00308         kni->unBlock();
00309       }
00310 
00311       idleWait.sleep();
00312     }
00313 
00315     //{
00316     //  assert(traj->at(0).splines.size() == NUM_JOINTS);
00317     //
00318     //  boost::recursive_mutex::scoped_lock lock(kni_mutex);
00319     //  std::vector<int> encoders;
00320     //
00321     //  for (size_t i = 0; i < NUM_JOINTS; i++) {
00322     //    encoders.push_back(converter->angle_rad2enc(i, traj->at(0).splines[i].coef[0]));
00323     //  }
00324     //
00325     //  std::vector<int> current_encoders = kni->getRobotEncoders(true);
00326     //  ROS_INFO("current encoders: %d %d %d %d %d", current_encoders[0], current_encoders[1], current_encoders[2], current_encoders[3], current_encoders[4]);
00327     //  ROS_INFO("target encoders:  %d %d %d %d %d", encoders[0], encoders[1], encoders[2], encoders[3], encoders[4]);
00328     //
00329     //  kni->moveRobotToEnc(encoders, false);
00330     //  ros::Duration(2.0).sleep();
00331     //}
00332 
00333     // ------- wait until start time
00334     ros::Time start_time = ros::Time(traj->at(0).start_time);
00335     double time_until_start = (start_time - ros::Time::now()).toSec();
00336 
00337     if (fabs(traj->at(0).start_time) > 0.01 && time_until_start < -0.01)
00338     {
00339       // only print warning if traj->at(0).start_time != 0 (MoveIt usually doesn't set start time)
00340       ROS_WARN("Trajectory started %f s too late! Scheduled: %f, started: %f", -time_until_start, start_time.toSec(), ros::Time::now().toSec());
00341     }
00342     else if (time_until_start > 0.0)
00343     {
00344       ROS_DEBUG("Sleeping %f seconds until scheduled start of trajectory", time_until_start);
00345       ros::Time::sleepUntil(start_time);
00346     }
00347 
00348     // ------- start trajectory
00349     boost::recursive_mutex::scoped_lock lock(kni_mutex);
00350 
00351     // fix start times: set the trajectory start time to now(); since traj is a shared pointer,
00352     // this fixes the current_trajectory_ in joint_trajectory_action_controller, which synchronizes
00353     // the "state" publishing to the actual start time (more or less)
00354     double delay = ros::Time::now().toSec() - traj->at(0).start_time;
00355     for (size_t i = 0; i < traj->size(); i++)
00356     {
00357       traj->at(i).start_time += delay;
00358     }
00359 
00360     for (size_t i = 0; i < traj->size(); i++)
00361     {
00362       Segment seg = traj->at(i);
00363       if (seg.splines.size() != joint_names_.size())
00364       {
00365         ROS_ERROR("Wrong number of joints in specified trajectory (was: %zu, expected: %zu)!", seg.splines.size(), joint_names_.size());
00366       }
00367 
00368       // set and start movement
00369       int activityflag = 0;
00370       if (i == (traj->size() - 1))
00371       {
00372         // last spline, start movement
00373         activityflag = 1; // no_next
00374       }
00375       else if (seg.start_time - traj->at(0).start_time < 0.4)
00376       {
00377         // more splines following, do not start movement yet
00378         activityflag = 2; // no_start
00379       }
00380       else
00381       {
00382         // more splines following, start movement
00383         activityflag = 0;
00384       }
00385 
00386       std::vector<short> polynomial;
00387       short s_time = round(seg.duration * KNI_TO_ROS_TIME);
00388       if (s_time <= 0)
00389         s_time = 1;
00390 
00391       for (size_t j = 0; j < seg.splines.size(); j++)
00392       {
00393         // some parts taken from CLMBase::movLM2P
00394         polynomial.push_back(s_time); // duration
00395 
00396         polynomial.push_back(converter->angle_rad2enc(j, seg.splines[j].target_position)); // target position
00397 
00398         // the four spline coefficients
00399         // the actual position, round
00400         polynomial.push_back(round(converter->angle_rad2enc(j, seg.splines[j].coef[0]))); // p0
00401 
00402         // shift to be firmware compatible and round
00403         polynomial.push_back(round(64 * converter->vel_rad2enc(j, seg.splines[j].coef[1]))); // p1
00404         polynomial.push_back(round(1024 * converter->acc_rad2enc(j, seg.splines[j].coef[2]))); // p2
00405         polynomial.push_back(round(32768 * converter->jerk_rad2enc(j, seg.splines[j].coef[3]))); // p3
00406       }
00407 
00408       // gripper: hold current position
00409       polynomial.push_back(s_time); // duration
00410       polynomial.push_back(converter->angle_rad2enc(5, motor_angles_[5])); // target position (angle)
00411       polynomial.push_back(converter->angle_rad2enc(5, motor_angles_[5])); // p0
00412       polynomial.push_back(0); // p1
00413       polynomial.push_back(0); // p2
00414       polynomial.push_back(0); // p3
00415 
00416       ROS_DEBUG("setAndStartPolyMovement(%d): ", activityflag);
00417 
00418       for (size_t k = 5; k < polynomial.size(); k += 6)
00419       {
00420         ROS_DEBUG("   time: %d   target: %d   p0: %d   p1: %d   p2: %d   p3: %d",
00421             polynomial[k-5], polynomial[k-4], polynomial[k-3], polynomial[k-2], polynomial[k-1], polynomial[k]);
00422       }
00423 
00424       kni->setAndStartPolyMovement(polynomial, false, activityflag);
00425     }
00426     return true;
00427   }
00428   catch (const WrongCRCException &e)
00429   {
00430     ROS_ERROR("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)", e.message().c_str());
00431   }
00432   catch (const ReadNotCompleteException &e)
00433   {
00434     ROS_ERROR("ReadNotCompleteException: Another program accessed the KNI. Please stop it and restart the Katana node. (exception in executeTrajectory(): %s)", e.message().c_str());
00435   }
00436   catch (const FirmwareException &e)
00437   {
00438     // TODO: find out what the real cause of this is when it happens again
00439     // the message returned by the Katana is:
00440     // FirmwareException : 'StopperThread: collision on axis: 1 (axis N)'
00441     ROS_ERROR("FirmwareException: Motor collision? Perhaps we tried to send a trajectory that the arm couldn't follow. (exception in executeTrajectory(): %s)", e.message().c_str());
00442   }
00443   catch (const Exception &e)
00444   {
00445     ROS_ERROR("Unhandled exception in executeTrajectory(): %s", e.message().c_str());
00446   }
00447   catch (...)
00448   {
00449     ROS_ERROR("Unhandled exception in executeTrajectory()");
00450   }
00451   return false;
00452 }
00453 
00454 void Katana::freezeRobot()
00455 {
00456   boost::recursive_mutex::scoped_lock lock(kni_mutex);
00457   kni->flushMoveBuffers();
00458   kni->freezeRobot();
00459 }
00460 
00461 bool Katana::moveJoint(int motorIndex, double desiredAngle)
00462 {
00463   if (desiredAngle < motor_limits_[motorIndex].min_position || motor_limits_[motorIndex].max_position < desiredAngle)
00464   {
00465     ROS_ERROR("Desired angle %f is out of range [%f, %f]", desiredAngle, motor_limits_[motorIndex].min_position, motor_limits_[motorIndex].max_position);
00466     return false;
00467   }
00468 
00469   try
00470   {
00471     boost::recursive_mutex::scoped_lock lock(kni_mutex);
00472     kni->moveMotorToEnc(motorIndex, converter->angle_rad2enc(motorIndex, desiredAngle), false, 100);
00473     return true;
00474   }
00475   catch (const WrongCRCException &e)
00476   {
00477     ROS_ERROR("WrongCRCException: Two threads tried to access the KNI at once. This means that the locking in the Katana node is broken. (exception in moveJoint(): %s)", e.message().c_str());
00478   }
00479   catch (const ReadNotCompleteException &e)
00480   {
00481     ROS_ERROR("ReadNotCompleteException: Another program accessed the KNI. Please stop it and restart the Katana node. (exception in moveJoint(): %s)", e.message().c_str());
00482   }
00483   catch (const Exception &e)
00484   {
00485     ROS_ERROR("Unhandled exception in moveJoint(): %s", e.message().c_str());
00486   }
00487   catch (...)
00488   {
00489     ROS_ERROR("Unhandled exception in moveJoint()");
00490   }
00491   return false;
00492 }
00493 
00494 bool Katana::someMotorCrashed()
00495 {
00496   for (size_t i = 0; i < NUM_MOTORS; i++)
00497   {
00498     if (motor_status_[i] == MSF_MOTCRASHED)
00499       return true;
00500   }
00501 
00502   return false;
00503 }
00504 
00505 bool Katana::allJointsReady()
00506 {
00507   for (size_t i = 0; i < NUM_JOINTS; i++)
00508   {
00509     if ((motor_status_[i] != MSF_DESPOS) && (motor_status_[i] != (MSF_NLINMOV)))
00510       return false;
00511   }
00512 
00513   return true;
00514 }
00515 
00516 bool Katana::allMotorsReady()
00517 {
00518   for (size_t i = 0; i < NUM_MOTORS; i++)
00519   {
00520     if ((motor_status_[i] != MSF_DESPOS) && (motor_status_[i] != (MSF_NLINMOV)))
00521       return false;
00522   }
00523 
00524   return true;
00525 }
00526 
00527 /* ******************************** helper functions ******************************** */
00528 
00532 short inline Katana::round(const double x)
00533 {
00534   if (x >= 0)
00535     return (short)(x + 0.5);
00536   else
00537     return (short)(x - 0.5);
00538 }
00539 
00540 void Katana::calibrate()
00541 {
00542   // private function only called in constructor, so no locking required
00543   bool calibrate = false;
00544   const int encoders = 100;
00545 
00546   kni->unBlock();
00547 
00548   // check if gripper collides in both cases (open and close gripper)
00549   // note MG: "1" is not the gripper!
00550   kni->enableCrashLimits();
00551 
00552   try
00553   {
00554     kni->moveMotorByEnc(1, encoders);
00555     ROS_INFO("no calibration required");
00556   }
00557   catch (...)
00558   {
00559     ROS_INFO("first calibration collision... ");
00560     try
00561     {
00562       kni->moveMotorByEnc(1, -encoders);
00563       ROS_INFO("no calibration required");
00564     }
00565     catch (...)
00566     {
00567       ROS_INFO("second calibration collision: calibration required");
00568       calibrate = true;
00569     }
00570   }
00571 
00572   if (calibrate)
00573   {
00574     kni->disableCrashLimits();
00575     kni->calibrate();
00576     kni->enableCrashLimits();
00577   }
00578 }
00579 
00584 bool Katana::switchMotorsOff(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response)
00585 {
00586   ROS_WARN("Switching all motors off!");
00587   boost::recursive_mutex::scoped_lock lock(kni_mutex);
00588   kni->switchRobotOff();
00589   return true;
00590 }
00591 
00592 bool Katana::switchMotorsOn(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response)
00593 {
00594   ROS_INFO("Switching all motors back on.");
00595   boost::recursive_mutex::scoped_lock lock(kni_mutex);
00596   kni->switchRobotOn();
00597   return true;
00598 }
00599 
00600 bool Katana::testSpeedSrv(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response)
00601 {
00602   testSpeed();
00603   return true;
00604 }
00605 
00606 void Katana::testSpeed()
00607 {
00608   ros::Rate idleWait(5);
00609   std::vector<double> pos1_angles(NUM_MOTORS);
00610   std::vector<double> pos2_angles(NUM_MOTORS);
00611 
00612   // these are safe values, i.e., no self-collision is possible
00613   pos1_angles[0] = 2.88;
00614   pos2_angles[0] = -3.02;
00615 
00616   pos1_angles[1] = 0.15;
00617   pos2_angles[1] = 2.16;
00618 
00619   pos1_angles[2] = 1.40;
00620   pos2_angles[2] = -2.21;
00621 
00622   pos1_angles[3] = 0.50;
00623   pos2_angles[3] = -2.02;
00624 
00625   pos1_angles[4] = 2.86;
00626   pos2_angles[4] = -2.98;
00627 
00628   pos1_angles[5] = -0.44;
00629   pos2_angles[5] = 0.30;
00630 
00631   for (size_t i = 0; i < NUM_MOTORS; i++)
00632   {
00633     int pos1_encoders = (int)converter->angle_rad2enc(i, pos1_angles[i]);
00634     int pos2_encoders = (int)converter->angle_rad2enc(i, pos2_angles[i]);
00635 
00636     int accel = kni->getMotorAccelerationLimit(i);
00637     int max_vel = kni->getMotorVelocityLimit(i);
00638 
00639     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));
00640     ROS_INFO("KNI encoders: %d, %d", kni->GetBase()->GetMOT()->arr[i].GetEncoderMinPos(), kni->GetBase()->GetMOT()->arr[i].GetEncoderMaxPos());
00641     ROS_INFO("moving to encoders: %d, %d", pos1_encoders, pos2_encoders);
00642     ROS_INFO("current encoders: %d", kni->getMotorEncoders(i, true));
00643 
00644     ROS_INFO("Moving to min");
00645     {
00646       boost::recursive_mutex::scoped_lock lock(kni_mutex);
00647       kni->moveMotorToEnc(i, pos1_encoders);
00648     }
00649 
00650     do
00651     {
00652       idleWait.sleep();
00653       refreshMotorStatus();
00654     } while (!allMotorsReady());
00655 
00656     ROS_INFO("Moving to max");
00657     {
00658       boost::recursive_mutex::scoped_lock lock(kni_mutex);
00659       kni->moveMotorToEnc(i, pos2_encoders);
00660     }
00661 
00662     do
00663     {
00664       idleWait.sleep();
00665       refreshMotorStatus();
00666     } while (!allMotorsReady());
00667   }
00668 
00669   // Result:
00670   //  Motor 0 - acceleration: 2 (= -4.908739), max speed: 180 (=-2.208932)
00671   //  Motor 1 - acceleration: 2 (= -2.646220), max speed: 180 (=-1.190799)
00672   //  Motor 2 - acceleration: 2 (= 5.292440), max speed: 180 (=2.381598)
00673   //     --> wrong! the measured values are more like 2.6, 1.2
00674   //
00675   //  Motor 3 - acceleration: 2 (= -4.908739), max speed: 180 (=-2.208932)
00676   //  Motor 4 - acceleration: 2 (= -4.908739), max speed: 180 (=-2.208932)
00677   //  Motor 5 - acceleration: 2 (= 1.597410), max speed: 180 (=0.718834)
00678   //     (TODO: the gripper duration can be calculated from this)
00679 }
00680 
00681 }


katana
Author(s): Martin Günther
autogenerated on Thu Aug 27 2015 13:40:35