00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027 #include "Kuka160nAxesVelocityController.hpp"
00028 #include <ocl/ComponentLoader.hpp>
00029
00030 #include <rtt/Logger.hpp>
00031
00032
00033 namespace OCL{
00034 using namespace RTT;
00035 using namespace std;
00036 using namespace KDL;
00037
00038 #define KUKA160_NUM_AXES 6
00039 #define KUKA160_CONV1 -120*114*106*100/( 30*40*48*14)
00040 #define KUKA160_CONV2 -168*139*111/(28*37*15)
00041 #define KUKA160_CONV3 -168*125*106/(28*41*15)
00042 #define KUKA160_CONV4 -150.857
00043 #define KUKA160_CONV5 -155.17
00044 #define KUKA160_CONV6 -100
00045
00046
00047 #define KUKA160_ENC_RES 4096
00048
00049
00050 #define KUKA160_TICKS2RAD { 2*M_PI / (KUKA160_CONV1 * KUKA160_ENC_RES), 2*M_PI / (KUKA160_CONV2 * KUKA160_ENC_RES), 2*M_PI / (KUKA160_CONV3 * KUKA160_ENC_RES), 2*M_PI / (KUKA160_CONV4 * KUKA160_ENC_RES), 2*M_PI / (KUKA160_CONV5 * KUKA160_ENC_RES), 2*M_PI / (KUKA160_CONV6 * KUKA160_ENC_RES)}
00051
00052
00053 #define KUKA160_RADproSEC2VOLT { -3.97143, -4.40112, -3.65062, -3.38542, -4.30991, -2.75810 }
00054
00055 typedef Kuka160nAxesVelocityController MyType;
00056
00057 Kuka160nAxesVelocityController::Kuka160nAxesVelocityController(string name)
00058 : TaskContext(name,PreOperational),
00059 driveValues_port("nAxesOutputVelocity"),
00060 referenceValues_port("ReferenceValues"),
00061 positionValues_port("nAxesSensorPosition"),
00062 driveLimits_prop("driveLimits","velocity limits of the axes, (rad/s)",std::vector<double>(KUKA160_NUM_AXES,0)),
00063 lowerPositionLimits_prop("LowerPositionLimits","Lower position limits (rad)",std::vector<double>(KUKA160_NUM_AXES,0)),
00064 upperPositionLimits_prop("UpperPositionLimits","Upper position limits (rad)",std::vector<double>(KUKA160_NUM_AXES,0)),
00065 initialPosition_prop("initialPosition","Initial position (rad) for simulation or hardware",std::vector<double>(KUKA160_NUM_AXES,0)),
00066 driveOffset_prop("driveOffset","offset (in rad/s) to the drive value.",std::vector<double>(KUKA160_NUM_AXES,0)),
00067 simulation_prop("simulation","true if simulationAxes should be used",true),
00068 simulation(true),
00069 EmergencyEvents_prop("EmergencyEvents","List of events that will result in an emergencystop of the robot"),
00070 num_axes_attr("KUKA160_NUM_AXES",KUKA160_NUM_AXES),
00071 chain_attr("Kinematics"),
00072 driveOutOfRange_evt("driveOutOfRange"),
00073 positionOutOfRange_evt("positionOutOfRange"),
00074 activated(false),
00075 positionConvertFactor(KUKA160_NUM_AXES),
00076 driveConvertFactor(KUKA160_NUM_AXES),
00077 driveValues(KUKA160_NUM_AXES),
00078 positionValues(KUKA160_NUM_AXES),
00079 references(KUKA160_NUM_AXES),
00080 #if (defined OROPKG_OS_LXRT)
00081 encoderInterface(KUKA160_NUM_AXES),
00082 vref(KUKA160_NUM_AXES),
00083 encoder(KUKA160_NUM_AXES),
00084 enable(KUKA160_NUM_AXES),
00085 drive(KUKA160_NUM_AXES),
00086 brake(KUKA160_NUM_AXES),
00087 reference(KUKA160_NUM_AXES),
00088 axes_hardware(KUKA160_NUM_AXES),
00089 #endif
00090 axes(KUKA160_NUM_AXES),
00091 axes_simulation(KUKA160_NUM_AXES)
00092 {
00093 Logger::In in(this->getName().data());
00094
00095 double ticks2rad[KUKA160_NUM_AXES] = KUKA160_TICKS2RAD;
00096 double vel2volt[KUKA160_NUM_AXES] = KUKA160_RADproSEC2VOLT;
00097 for(unsigned int i = 0;i<KUKA160_NUM_AXES;i++){
00098 positionConvertFactor[i] = ticks2rad[i];
00099 driveConvertFactor[i] = vel2volt[i];
00100 }
00101
00102 #if (defined OROPKG_OS_LXRT)
00103 comediDevAOut = new ComediDevice( 0 );
00104 comediDevDInOut = new ComediDevice( 3 );
00105 comediDevEncoder = new ComediDevice( 2 );
00106 log(Info)<< "ComediDevices Created"<<endlog();
00107
00108 int subd;
00109 subd = 1;
00110 comediSubdevAOut = new ComediSubDeviceAOut( comediDevAOut, "Kuka160", subd );
00111 subd = 0;
00112 comediSubdevDIn = new ComediSubDeviceDIn( comediDevDInOut, "Kuka160", subd );
00113 subd = 1;
00114 comediSubdevDOut = new ComediSubDeviceDOut( comediDevDInOut, "Kuka160", subd );
00115 log(Info)<<"ComediSubDevices Created"<<endlog();
00116
00117
00118 for(int i = 0; i < 24 ; i++) comediSubdevDOut->switchOff( i );
00119
00120 for (unsigned int i = 0; i < KUKA160_NUM_AXES; i++){
00121 log(Info)<<"Kuka160 Creating Hardware Axis "<<i<<endlog();
00122
00123 log(Info)<<"Setting up encoder ..."<<endlog();
00124 subd = 0;
00125 encoderInterface[i] = new ComediEncoder(comediDevEncoder , subd , i);
00126 log(Info)<<"Creation ComediEncoder succeeded."<<endlog();
00127
00128 log(Info)<<"encoder settings: "<<positionConvertFactor[i]<<endlog();
00129 log(Info)<<", "<<initialPosition_prop.value()[i]<<endlog();
00130 log(Info)<<", "<<lowerPositionLimits_prop.value()[i]<<endlog();
00131 log(Info)<<", "<<upperPositionLimits_prop.value()[i]<<endlog();
00132
00133 encoder[i] = new IncrementalEncoderSensor( encoderInterface[i], 1.0 / positionConvertFactor[i],
00134 initialPosition_prop.value()[i]*positionConvertFactor[i],
00135 lowerPositionLimits_prop.value()[i], upperPositionLimits_prop.value()[i],KUKA160_ENC_RES);
00136 log(Info)<<"Setting up brake ..."<<endlog();
00137 brake[i] = new DigitalOutput( comediSubdevDOut, 23 - i,true);
00138 brake[i]->switchOn();
00139
00140 vref[i] = new AnalogOutput( comediSubdevAOut, i );
00141 enable[i] = new DigitalOutput( comediSubdevDOut, 13 - i );
00142 reference[i] = new DigitalInput( comediSubdevDIn, 23 - i);
00143 drive[i] = new AnalogDrive( vref[i], enable[i], 1.0 / vel2volt[i], driveOffset_prop.value()[i]);
00144
00145 axes_hardware[i] = new Axis( drive[i] );
00146 axes_hardware[i]->setBrake( brake[i] );
00147 axes_hardware[i]->setSensor( "Position", encoder[i] );
00148 axes_hardware[i]->setSwitch( "Reference", reference[i]);
00149
00150 }
00151
00152 #endif
00153
00154 kinematics.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.0,0.9))));
00155 kinematics.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.97))));
00156 kinematics.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,1.080))));
00157 kinematics.addSegment(Segment(Joint(Joint::RotZ)));
00158 kinematics.addSegment(Segment(Joint(Joint::RotX)));
00159 kinematics.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.0,0.18))));
00160
00161 chain_attr.set(kinematics);
00162
00163
00164
00165
00166
00167
00168 this->methods()->addMethod( RTT::method("startAllAxes",&MyType::startAllAxes,this),
00169 "start all axes" );
00170 this->methods()->addMethod( RTT::method("stopAllAxes",&MyType::stopAllAxes,this),
00171 "stops all axes" );
00172 this->methods()->addMethod( RTT::method("lockAllAxes",&MyType::lockAllAxes,this),
00173 "locks all axes" );
00174 this->methods()->addMethod( RTT::method("unlockAllAxes",&MyType::unlockAllAxes,this),
00175 "unlock all axes" );
00176 this->methods()->addMethod( RTT::method("addDriveOffset",&MyType::addDriveOffset,this),
00177 "adds offset to the drive values",
00178 "offset","offset value in rad/s" );
00179 this->methods()->addMethod( RTT::method("initPosition",&MyType::initPosition,this),
00180 "changes position value of axis to the initial position",
00181 "switchposition","recorded switchpositions");
00182
00183 this->commands()->addCommand(RTT::command("prepareForUse",&MyType::prepareForUse,&MyType::prepareForUseCompleted,this),
00184 "prepares the robot for use" );
00185 this->commands()->addCommand(RTT::command("prepareForShutdown",&MyType::prepareForShutdown,&MyType::prepareForShutdownCompleted,this),
00186 "prepares the robot for shutdown" );
00187
00191 ports()->addPort(&driveValues_port);
00192 ports()->addPort(&positionValues_port);
00193 ports()->addPort(&referenceValues_port);
00194
00198 events()->addEvent( &driveOutOfRange_evt, "Velocity of Axis is out of range","message","Information about event" );
00199 events()->addEvent( &positionOutOfRange_evt, "Position of an Axis is out of range","message","Information about event");
00200
00201
00202 properties()->addProperty( &driveLimits_prop );
00203 properties()->addProperty( &lowerPositionLimits_prop );
00204 properties()->addProperty( &upperPositionLimits_prop );
00205 properties()->addProperty( &initialPosition_prop );
00206 properties()->addProperty( &driveOffset_prop );
00207 properties()->addProperty( &simulation_prop );
00208 properties()->addProperty( &EmergencyEvents_prop);
00209 attributes()->addConstant( &num_axes_attr);
00210 attributes()->addAttribute(&chain_attr);
00211
00212
00213
00214
00215 }
00216
00217 Kuka160nAxesVelocityController::~Kuka160nAxesVelocityController()
00218 {
00219 this->cleanup();
00220
00221
00222 if(simulation)
00223 for (unsigned int i = 0; i < KUKA160_NUM_AXES; i++)
00224 delete axes[i];
00225
00226 #if (defined OROPKG_OS_LXRT)
00227 for (unsigned int i = 0; i < KUKA160_NUM_AXES; i++)
00228 delete axes_hardware[i];
00229 delete comediDevAOut;
00230 delete comediDevDInOut;
00231 delete comediDevEncoder;
00232 delete comediSubdevAOut;
00233 delete comediSubdevDIn;
00234 delete comediSubdevDOut;
00235 #endif
00236 }
00237 bool Kuka160nAxesVelocityController::configureHook()
00238 {
00239 Logger::In in(this->getName().data());
00240
00241 simulation=simulation_prop.value();
00242
00243 if(!(driveLimits_prop.value().size()==KUKA160_NUM_AXES&&
00244 lowerPositionLimits_prop.value().size()==KUKA160_NUM_AXES&&
00245 upperPositionLimits_prop.value().size()==KUKA160_NUM_AXES&&
00246 initialPosition_prop.value().size()==KUKA160_NUM_AXES&&
00247 driveOffset_prop.value().size()==KUKA160_NUM_AXES))
00248 return false;
00249
00250 #if (defined OROPKG_OS_LXRT)
00251 if(!simulation){
00252 for (unsigned int i = 0; i <KUKA160_NUM_AXES; i++){
00253 axes_hardware[i]->limitDrive(-driveLimits_prop.value()[i], driveLimits_prop.value()[i], driveOutOfRange_evt);
00254 axes[i] = axes_hardware[i];
00255 ((Axis*)(axes[i]))->getDrive()->addOffset(driveOffset_prop.value()[i]);
00256 }
00257 log(Info) << "Hardware version of Kuka160nAxesVelocityController has started" << endlog();
00258 }
00259 else{
00260 #endif
00261 for (unsigned int i = 0; i <KUKA160_NUM_AXES; i++)
00262 axes[i] = new SimulationAxis(initialPosition_prop.value()[i],
00263 lowerPositionLimits_prop.value()[i],
00264 upperPositionLimits_prop.value()[i]);
00265 log(Info) << "Simulation version of Kuka160nAxesVelocityController has started" << endlog();
00266 #if (defined OROPKG_OS_LXRT)
00267 }
00268 #endif
00269 for(unsigned int i=0;i<EmergencyEvents_prop.value().size();i++){
00270 string name = EmergencyEvents_prop.value()[i];
00271 string::size_type idx = name.find('.');
00272 if(idx==string::npos)
00273 log(Warning)<<"Could not connect EmergencyStop to "<<name<<"\n Syntax of "
00274 <<name<<" is not correct. I want a ComponentName.EventName "<<endlog();
00275 string peername = name.substr(0,idx);
00276 string eventname = name.substr(idx+1);
00277 TaskContext* peer;
00278 if(peername==this->getName())
00279 peer = this;
00280 else if(this->hasPeer(peername)){
00281 peer = this->getPeer(peername);
00282 }else{
00283 log(Warning)<<"Could not connect EmergencyStop to "<<name<<", "<<peername<<" is not a peer of "<<this->getName()<<endlog();
00284 continue;
00285 }
00286
00287 if(peer->events()->hasEvent(eventname)){
00288 Handle handle = peer->events()->setupConnection(eventname).callback(this,&Kuka160nAxesVelocityController::EmergencyStop).handle();
00289 if(handle.connect()){
00290 EmergencyEventHandlers.push_back(handle);
00291 log(Info)<<"EmergencyStop connected to "<< name<<" event."<<endlog();
00292 }else
00293 log(Warning)<<"Could not connect EmergencyStop to "<<name<<", "<<eventname<<" has to have a message parameter."<<endlog();
00294 }else
00295 log(Warning)<<"Could not connect EmergencyStop to "<<name<<", "<<eventname <<" not found in "<<peername<<"s event-list"<<endlog();
00296 }
00297 return true;
00298 }
00299
00300 bool Kuka160nAxesVelocityController::startHook()
00301 {
00302 return true;
00303 }
00304
00305 void Kuka160nAxesVelocityController::updateHook()
00306 {
00307
00308 driveValues_port.Get(driveValues);
00309
00310 for (int axis=0;axis<KUKA160_NUM_AXES;axis++) {
00311
00312 positionValues[axis]=axes[axis]->getSensor("Position")->readSensor();
00313
00314
00315 if( (positionValues[axis] < lowerPositionLimits_prop.value()[axis]) ||
00316 (positionValues[axis] > upperPositionLimits_prop.value()[axis]) ){
00317 char msg[80];
00318 sprintf(msg,"Position of Kuka160 Axis %d is out of range: value: %f",axis,positionValues[axis]);
00319 positionOutOfRange_evt(msg);
00320 }
00321
00322
00323 if (axes[axis]->isDriven())
00324 axes[axis]->drive(driveValues[axis]);
00325
00326
00327 #if (defined OROPKG_OS_LXRT)
00328 if(!simulation)
00329 references[axis]=axes[axis]->getSwitch("Reference")->isOn();
00330 else
00331 #endif
00332 references[axis]=false;
00333 }
00334 positionValues_port.Set(positionValues);
00335 referenceValues_port.Set(references);
00336
00337 }
00338
00339 void Kuka160nAxesVelocityController::stopHook()
00340 {
00341
00342 prepareForShutdown();
00343 }
00344
00345 void Kuka160nAxesVelocityController::cleanupHook()
00346 {
00347 }
00348
00349 bool Kuka160nAxesVelocityController::prepareForUse()
00350 {
00351 #if (defined OROPKG_OS_LXRT)
00352 if(!simulation){
00353 comediSubdevDOut->switchOn( 17 );
00354 log(Warning) <<"Release Emergency stop of Kuka 160 and push button to start ...."<<endlog();
00355 }
00356 #endif
00357 activated = true;
00358 return true;
00359 }
00360
00361 bool Kuka160nAxesVelocityController::prepareForUseCompleted()const
00362 {
00363 #if (defined OROPKG_OS_LXRT)
00364 if(!simulation)
00365 return (comediSubdevDIn->isOn(3) && comediSubdevDIn->isOn(5));
00366 #endif
00367 return true;
00368 }
00369
00370 bool Kuka160nAxesVelocityController::prepareForShutdown()
00371 {
00372
00373 stopAllAxes();
00374 lockAllAxes();
00375 #if (defined OROPKG_OS_LXRT)
00376 if(!simulation)
00377 comediSubdevDOut->switchOff( 17 );
00378 #endif
00379 activated = false;
00380 return true;
00381 }
00382
00383 bool Kuka160nAxesVelocityController::prepareForShutdownCompleted()const
00384 {
00385 return true;
00386 }
00387
00388 bool Kuka160nAxesVelocityController::stopAllAxes()
00389 {
00390 bool succes = true;
00391 for(unsigned int i = 0;i<KUKA160_NUM_AXES;i++)
00392 succes &= axes[i]->stop();
00393
00394 return succes;
00395 }
00396
00397 bool Kuka160nAxesVelocityController::startAllAxes()
00398 {
00399 bool succes = true;
00400 for(unsigned int i = 0;i<KUKA160_NUM_AXES;i++)
00401 succes &= axes[i]->drive(0.0);
00402
00403 return succes;
00404 }
00405
00406 bool Kuka160nAxesVelocityController::unlockAllAxes()
00407 {
00408 if(!activated)
00409 return false;
00410
00411 bool succes = true;
00412 for(unsigned int i = 0;i<KUKA160_NUM_AXES;i++)
00413 succes &= axes[i]->unlock();
00414
00415 return succes;
00416 }
00417
00418 bool Kuka160nAxesVelocityController::lockAllAxes()
00419 {
00420 bool succes = true;
00421 for(unsigned int i = 0;i<KUKA160_NUM_AXES;i++){
00422 succes &= axes[i]->lock();
00423 }
00424 return succes;
00425 }
00426
00427
00428 bool Kuka160nAxesVelocityController::addDriveOffset(const std::vector<double>& offset)
00429 {
00430 if(offset.size()!=KUKA160_NUM_AXES)
00431 return false;
00432 for(unsigned int i=0;i<KUKA160_NUM_AXES;i++){
00433 driveOffset_prop.value()[i] += offset[i];
00434
00435 #if (defined OROPKG_OS_LXRT)
00436 if (!simulation)
00437 ((Axis*)(axes[i]))->getDrive()->addOffset(offset[i]);
00438 #endif
00439 }
00440
00441 return true;
00442 }
00443
00444 bool Kuka160nAxesVelocityController::initPosition(const std::vector<double>& switchposition)
00445 {
00446 #if (defined OROPKG_OS_LXRT)
00447 if(!simulation)
00448 for(unsigned int i=0;i<KUKA160_NUM_AXES;i++){
00449 double act_pos = ((IncrementalEncoderSensor*)axes[i]->getSensor("Position"))->readSensor();
00450 double new_pos = act_pos-switchposition[i]+initialPosition_prop.value()[i];
00451 ((IncrementalEncoderSensor*)axes[i]->getSensor("Position"))->writeSensor(new_pos);
00452 }
00453 #endif
00454 return true;
00455 }
00456
00457 }
00458
00459 ORO_LIST_COMPONENT_TYPE( OCL::Kuka160nAxesVelocityController );
00460
