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 "Kuka361nAxesVelocityController.hpp"
00028 #include <ocl/ComponentLoader.hpp>
00029
00030 #include <rtt/Logger.hpp>
00031 #include <stdlib.h>
00032
00033 namespace OCL
00034 {
00035 using namespace RTT;
00036 using namespace KDL;
00037 using namespace std;
00038
00039 #define KUKA361_NUM_AXES 6
00040
00041
00042
00043
00044 #define KUKA361_ENCODEROFFSETS { 1000004, 1000000, 502652, 1001598, 985928, 1230656 }
00045
00046 #define KUKA361_CONV1 94.14706
00047 #define KUKA361_CONV2 -103.23529
00048 #define KUKA361_CONV3 51.44118
00049 #define KUKA361_CONV4 175
00050 #define KUKA361_CONV5 150
00051 #define KUKA361_CONV6 131.64395
00052
00053 #define KUKA361_ENC_RES 4096
00054
00055
00056 #define KUKA361_TICKS2RAD { 2*M_PI / (KUKA361_CONV1 * KUKA361_ENC_RES), 2*M_PI / (KUKA361_CONV2 * KUKA361_ENC_RES), 2*M_PI / (KUKA361_CONV3 * KUKA361_ENC_RES), 2*M_PI / (KUKA361_CONV4 * KUKA361_ENC_RES), 2*M_PI / (KUKA361_CONV5 * KUKA361_ENC_RES), 2*M_PI / (KUKA361_CONV6 * KUKA361_ENC_RES)}
00057
00058
00059
00060 #define KUKA361_RADproSEC2VOLT { 2.5545, 2.67804024532652, 1.37350318088664, 2.34300679603342, 2.0058, 1.7573 } //24 april 2007
00061
00062 #define KINEMATICS_EPS 0.0001
00063 #define SQRT3d2 0.8660254037844386
00064 #define M_PI_T2 2 * M_PI
00065 #define SQRT3t2 3.46410161513775 // 2 sqrt(3)
00066
00067 typedef Kuka361nAxesVelocityController MyType;
00068
00069 Kuka361nAxesVelocityController::Kuka361nAxesVelocityController(string name)
00070 : TaskContext(name,PreOperational),
00071 startAllAxes_mtd( "startAllAxes", &MyType::startAllAxes, this),
00072 stopAllAxes_mtd( "stopAllAxes", &MyType::stopAllAxes, this),
00073 unlockAllAxes_mtd( "unlockAllAxes", &MyType::unlockAllAxes, this),
00074 lockAllAxes_mtd( "lockAllAxes", &MyType::lockAllAxes, this),
00075 prepareForUse_cmd( "prepareForUse", &MyType::prepareForUse,&MyType::prepareForUseCompleted, this),
00076 prepareForShutdown_cmd( "prepareForShutdown", &MyType::prepareForShutdown,&MyType::prepareForShutdownCompleted, this),
00077 addDriveOffset_mtd( "addDriveOffset", &MyType::addDriveOffset, this),
00078 driveValues_port("nAxesOutputVelocity"),
00079 positionValues_port("nAxesSensorPosition"),
00080 driveLimits_prop("driveLimits","velocity limits of the axes, (rad/s)",std::vector<double>(KUKA361_NUM_AXES,0)),
00081 lowerPositionLimits_prop("LowerPositionLimits","Lower position limits (rad)",std::vector<double>(KUKA361_NUM_AXES,0)),
00082 upperPositionLimits_prop("UpperPositionLimits","Upper position limits (rad)",std::vector<double>(KUKA361_NUM_AXES,0)),
00083 initialPosition_prop("initialPosition","Initial position (rad) for simulation or hardware",std::vector<double>(KUKA361_NUM_AXES,0)),
00084 driveOffset_prop("driveOffset","offset (in rad/s) to the drive value.",std::vector<double>(KUKA361_NUM_AXES,0)),
00085 simulation_prop("simulation","true if simulationAxes should be used",true),
00086 simulation(true),
00087 geometric_prop("geometric","true if drive and positions should be converted for kinematic use",true),
00088 EmergencyEvents_prop("EmergencyEvents","List of events that will result in an emergencystop of the robot"),
00089 num_axes_attr("NUM_AXES",KUKA361_NUM_AXES),
00090 chain_attr("Kinematics"),
00091 driveOutOfRange_evt("driveOutOfRange"),
00092 positionOutOfRange_evt("positionOutOfRange"),
00093 activated(false),
00094 positionConvertFactor(KUKA361_NUM_AXES),
00095 driveConvertFactor(KUKA361_NUM_AXES),
00096 positionValues(KUKA361_NUM_AXES),
00097 driveValues(KUKA361_NUM_AXES),
00098 positionValues_kin(KUKA361_NUM_AXES),
00099 driveValues_rob(KUKA361_NUM_AXES),
00100 #if (defined OROPKG_OS_LXRT)
00101 axes_hardware(KUKA361_NUM_AXES),
00102 encoderInterface(KUKA361_NUM_AXES),
00103 encoder(KUKA361_NUM_AXES),
00104 vref(KUKA361_NUM_AXES),
00105 enable(KUKA361_NUM_AXES),
00106 drive(KUKA361_NUM_AXES),
00107 brake(KUKA361_NUM_AXES),
00108 #endif
00109 axes(KUKA361_NUM_AXES)
00110 {
00111 Logger::In in(this->getName().data());
00112 double ticks2rad[KUKA361_NUM_AXES] = KUKA361_TICKS2RAD;
00113 double vel2volt[KUKA361_NUM_AXES] = KUKA361_RADproSEC2VOLT;
00114 for(unsigned int i = 0;i<KUKA361_NUM_AXES;i++){
00115 positionConvertFactor[i] = ticks2rad[i];
00116 driveConvertFactor[i] = vel2volt[i];
00117 }
00118
00119 #if (defined OROPKG_OS_LXRT)
00120 int encoderOffsets[KUKA361_NUM_AXES] = KUKA361_ENCODEROFFSETS;
00121
00122 log(Info)<<"Creating Comedi Devices."<<endlog();
00123 comediDev = new ComediDevice( 1 );
00124 comediSubdevAOut = new ComediSubDeviceAOut( comediDev, "Kuka361" );
00125 log(Info)<<"Creating APCI Devices."<<endlog();
00126 apci1710 = new EncoderSSI_apci1710_board( 0, 1 , 2);
00127 apci2200 = new RelayCardapci2200( "Kuka361" );
00128 apci1032 = new SwitchDigitalInapci1032( "Kuka361" );
00129
00130
00131
00132 encoderInterface[0] = new EncoderSSI_apci1710( 1, apci1710 );
00133 encoderInterface[1] = new EncoderSSI_apci1710( 2, apci1710 );
00134 encoderInterface[2] = new EncoderSSI_apci1710( 7, apci1710 );
00135 encoderInterface[3] = new EncoderSSI_apci1710( 8, apci1710 );
00136 encoderInterface[4] = new EncoderSSI_apci1710( 5, apci1710 );
00137 encoderInterface[5] = new EncoderSSI_apci1710( 6, apci1710 );
00138
00139
00140 for (unsigned int i = 0; i < KUKA361_NUM_AXES; i++){
00141 log(Info)<<"Creating Hardware axis "<<i<<endlog();
00142
00143 log(Info)<<"Setting up encoder ..."<<endlog();
00144 encoder[i] = new AbsoluteEncoderSensor( encoderInterface[i], 1.0 / ticks2rad[i], encoderOffsets[i], -10, 10 );
00145
00146 log(Info)<<"Setting up brake ..."<<endlog();
00147 brake[i] = new DigitalOutput( apci2200, i + KUKA361_NUM_AXES );
00148 log(Info)<<"Setting brake to on"<<endlog();
00149 brake[i]->switchOn();
00150
00151 log(Info)<<"Setting up drive ..."<<endlog();
00152 vref[i] = new AnalogOutput( comediSubdevAOut, i );
00153 enable[i] = new DigitalOutput( apci2200, i );
00154 drive[i] = new AnalogDrive( vref[i], enable[i], 1.0 / vel2volt[i], 0.0);
00155
00156 axes_hardware[i] = new Axis( drive[i] );
00157 axes_hardware[i]->setBrake( brake[i] );
00158 axes_hardware[i]->setSensor( "Position", encoder[i] );
00159 }
00160
00161 #endif
00162
00163 kinematics.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.0,1.020))));
00164 kinematics.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.480))));
00165 kinematics.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.645))));
00166 kinematics.addSegment(Segment(Joint(Joint::RotZ)));
00167 kinematics.addSegment(Segment(Joint(Joint::RotX)));
00168 kinematics.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.0,0.120))));
00169
00170 chain_attr.set(kinematics);
00171
00172
00173
00174
00175 methods()->addMethod( &startAllAxes_mtd, "start all axes" );
00176 methods()->addMethod( &stopAllAxes_mtd, "stops all axes" );
00177 methods()->addMethod( &lockAllAxes_mtd, "locks all axes" );
00178 methods()->addMethod( &unlockAllAxes_mtd, "unlock all axes" );
00179 commands()->addCommand( &prepareForUse_cmd, "prepares the robot for use" );
00180 commands()->addCommand( &prepareForShutdown_cmd,"prepares the robot for shutdown" );
00181 methods()->addMethod( &addDriveOffset_mtd,"adds an offset to the drive value of axis","offset","offset values in rad/s" );
00182 events()->addEvent( &driveOutOfRange_evt, "Velocity of an Axis is out of range","message","Information about event" );
00183 events()->addEvent( &positionOutOfRange_evt, "Position of an Axis is out of range","message","Information about event");
00184
00188 ports()->addPort(&driveValues_port);
00189 ports()->addPort(&positionValues_port);
00190
00194 properties()->addProperty( &driveLimits_prop);
00195 properties()->addProperty( &lowerPositionLimits_prop);
00196 properties()->addProperty( &upperPositionLimits_prop);
00197 properties()->addProperty( &initialPosition_prop);
00198 properties()->addProperty( &driveOffset_prop);
00199 properties()->addProperty( &simulation_prop);
00200 properties()->addProperty( &geometric_prop);
00201 properties()->addProperty( &EmergencyEvents_prop);
00202 attributes()->addConstant( &num_axes_attr);
00203 attributes()->addAttribute(&chain_attr);
00204
00205 }
00206
00207 Kuka361nAxesVelocityController::~Kuka361nAxesVelocityController()
00208 {
00209
00210 this->cleanup();
00211
00212
00213 if(simulation)
00214 for (unsigned int i = 0; i < KUKA361_NUM_AXES; i++)
00215 delete axes[i];
00216
00217 #if (defined OROPKG_OS_LXRT)
00218 for (unsigned int i = 0; i < KUKA361_NUM_AXES; i++)
00219 delete axes_hardware[i];
00220 delete comediDev;
00221 delete comediSubdevAOut;
00222 delete apci1710;
00223 delete apci2200;
00224 delete apci1032;
00225 #endif
00226 }
00227
00228 bool Kuka361nAxesVelocityController::configureHook()
00229 {
00230 Logger::In in(this->getName().data());
00231
00232 simulation=simulation_prop.value();
00233
00234 if(!(driveLimits_prop.value().size()==KUKA361_NUM_AXES&&
00235 lowerPositionLimits_prop.value().size()==KUKA361_NUM_AXES&&
00236 upperPositionLimits_prop.value().size()==KUKA361_NUM_AXES&&
00237 initialPosition_prop.value().size()==KUKA361_NUM_AXES&&
00238 driveOffset_prop.value().size()==KUKA361_NUM_AXES))
00239 return false;
00240
00241 #if (defined OROPKG_OS_LXRT)
00242 if(!simulation){
00243 for (unsigned int i = 0; i <KUKA361_NUM_AXES; i++){
00244 axes_hardware[i]->limitDrive(-driveLimits_prop.value()[i], driveLimits_prop.value()[i], driveOutOfRange_evt);
00245 axes[i] = axes_hardware[i];
00246 ((Axis*)(axes[i]))->getDrive()->addOffset(driveOffset_prop.value()[i]);
00247 }
00248 log(Info) << "Hardware version of Kuka361nAxesVelocityController has started" << endlog();
00249 }
00250 else{
00251 #endif
00252 for (unsigned int i = 0; i <KUKA361_NUM_AXES; i++)
00253 axes[i] = new SimulationAxis(initialPosition_prop.value()[i],
00254 lowerPositionLimits_prop.value()[i],
00255 upperPositionLimits_prop.value()[i]);
00256 log(Info) << "Simulation version of Kuka361nAxesVelocityController has started" << endlog();
00257 #if (defined OROPKG_OS_LXRT)
00258 }
00259 #endif
00260 for(unsigned int i=0;i<EmergencyEvents_prop.value().size();i++){
00261 string name = EmergencyEvents_prop.value()[i];
00262 string::size_type idx = name.find('.');
00263 if(idx==string::npos)
00264 log(Warning)<<"Could not connect EmergencyStop to "<<name<<"\n Syntax of "
00265 <<name<<" is not correct. I want a ComponentName.EventName "<<endlog();
00266 string peername = name.substr(0,idx);
00267 string eventname = name.substr(idx+1);
00268 TaskContext* peer;
00269 if(peername==this->getName())
00270 peer = this;
00271 else if(this->hasPeer(peername)){
00272 peer = this->getPeer(peername);
00273 }else{
00274 log(Warning)<<"Could not connect EmergencyStop to "<<name<<", "<<peername<<" is not a peer of "<<this->getName()<<endlog();
00275 continue;
00276 }
00277
00278 if(peer->events()->hasEvent(eventname)){
00279 Handle handle = peer->events()->setupConnection(eventname).callback(this,&Kuka361nAxesVelocityController::EmergencyStop).handle();
00280 if(handle.connect()){
00281 EmergencyEventHandlers.push_back(handle);
00282 log(Info)<<"EmergencyStop connected to "<< name<<" event."<<endlog();
00283 }else
00284 log(Warning)<<"Could not connect EmergencyStop to "<<name<<", "<<eventname<<" has to have a message parameter."<<endlog();
00285 }else
00286 log(Warning)<<"Could not connect EmergencyStop to "<<name<<", "<<eventname <<" not found in "<<peername<<"s event-list"<<endlog();
00287 }
00288 return true;
00289 }
00290
00291 bool Kuka361nAxesVelocityController::startHook()
00292 {
00293 return true;
00294 }
00295
00296 void Kuka361nAxesVelocityController::updateHook()
00297 {
00298 for (unsigned int axis=0;axis<KUKA361_NUM_AXES;axis++) {
00299
00300 positionValues[axis] = axes[axis]->getSensor("Position")->readSensor();
00301 }
00302
00303 driveValues_port.Get(driveValues);
00304
00305 if(geometric_prop.value())
00306 convertGeometric();
00307
00308 positionValues_port.Set(positionValues);
00309
00310 for (unsigned int axis=0;axis<KUKA361_NUM_AXES;axis++){
00311 if (axes[axis]->isDriven())
00312 axes[axis]->drive(driveValues[axis]);
00313
00314
00315 if( (positionValues[axis] < lowerPositionLimits_prop.value()[axis]) ||
00316 (positionValues[axis] > upperPositionLimits_prop.value()[axis]) )
00317 positionOutOfRange_evt("Position of a Kuka361 Axis is out of range");
00318
00319
00320 }
00321 }
00322
00323
00324 void Kuka361nAxesVelocityController::stopHook()
00325 {
00326
00327 prepareForShutdown();
00328 }
00329
00330 void Kuka361nAxesVelocityController::cleanupHook()
00331 {
00332
00333 }
00334
00335 bool Kuka361nAxesVelocityController::prepareForUse()
00336 {
00337 #if (defined OROPKG_OS_LXRT)
00338 if(!simulation){
00339 apci2200->switchOn( 12 );
00340 apci2200->switchOn( 14 );
00341 log(Warning) <<"Release Emergency stop of Kuka361 and push button to start ...."<<endlog();
00342 }
00343 #endif
00344 activated = true;
00345 return true;
00346 }
00347
00348 bool Kuka361nAxesVelocityController::prepareForUseCompleted()const
00349 {
00350 #if (defined OROPKG_OS_LXRT)
00351 if(!simulation)
00352 return (apci1032->isOn(12) && apci1032->isOn(14));
00353 else
00354 #endif
00355 return true;
00356 }
00357
00358 bool Kuka361nAxesVelocityController::prepareForShutdown()
00359 {
00360
00361 stopAllAxes();
00362 lockAllAxes();
00363 #if (defined OROPKG_OS_LXRT)
00364 if(!simulation){
00365 apci2200->switchOff( 12 );
00366 apci2200->switchOff( 14 );
00367 }
00368
00369 #endif
00370 activated = false;
00371 return true;
00372 }
00373
00374 bool Kuka361nAxesVelocityController::prepareForShutdownCompleted()const
00375 {
00376 return true;
00377 }
00378
00379 bool Kuka361nAxesVelocityController::stopAllAxes()
00380 {
00381 bool succes = true;
00382 for(unsigned int i = 0;i<KUKA361_NUM_AXES;i++)
00383 succes &= axes[i]->stop();
00384
00385 return succes;
00386 }
00387
00388 bool Kuka361nAxesVelocityController::startAllAxes()
00389 {
00390 bool succes = true;
00391 for(unsigned int i = 0;i<KUKA361_NUM_AXES;i++)
00392 succes &= axes[i]->drive(0.0);
00393
00394 return succes;
00395 }
00396
00397 bool Kuka361nAxesVelocityController::unlockAllAxes()
00398 {
00399 if(!activated)
00400 return false;
00401
00402 bool succes = true;
00403 for(unsigned int i = 0;i<KUKA361_NUM_AXES;i++)
00404 succes &= axes[i]->unlock();
00405
00406 return succes;
00407 }
00408
00409 bool Kuka361nAxesVelocityController::lockAllAxes()
00410 {
00411 bool succes = true;
00412 for(unsigned int i = 0;i<KUKA361_NUM_AXES;i++){
00413 succes &= axes[i]->lock();
00414 }
00415 return succes;
00416 }
00417
00418 bool Kuka361nAxesVelocityController::addDriveOffset(const std::vector<double>& offset)
00419 {
00420 if(offset.size()!=KUKA361_NUM_AXES)
00421 return false;
00422
00423 for(unsigned int i=0;i<KUKA361_NUM_AXES;i++){
00424 if(geometric_prop.value() && (i==0 || i==3 || i==5) )
00425 {
00426 driveOffset_prop.value()[i] -= offset[i];
00427 }
00428 else
00429 {
00430 driveOffset_prop.value()[i] += offset[i];
00431 }
00432 #if (defined OROPKG_OS_LXRT)
00433 if (!simulation)
00434 {
00435 if(geometric_prop.value() && (i==0 || i==3 || i==5) )
00436 ((Axis*)(axes[i]))->getDrive()->addOffset(-offset[i]);
00437 else
00438 ((Axis*)(axes[i]))->getDrive()->addOffset(offset[i]);
00439 }
00440 #endif
00441 }
00442 return true;
00443 }
00444
00445 void Kuka361nAxesVelocityController::convertGeometric()
00446 {
00447 positionValues[0] = -positionValues[0];
00448 positionValues[3] = -positionValues[3];
00449 positionValues[5] = -positionValues[5];
00450 driveValues[0] = -driveValues[0];
00451 driveValues[3] = -driveValues[3];
00452 driveValues[5] = -driveValues[5];
00453 for(unsigned int i = 0; i < 3 ; i++){
00454 positionValues_kin[i] = positionValues[i];
00455 driveValues_rob[i] = driveValues[i];
00456 }
00457
00458
00459 double c5 = cos(positionValues[4]);
00460 double s5 = sin(positionValues[4]);
00461 double c5_eq = (c5+3.)/4;
00462 double alpha;
00463
00464 if (positionValues[4]<-KINEMATICS_EPS){
00465 alpha = atan2(-s5,SQRT3d2*(c5-1.));
00466 positionValues_kin[4]=-2.*acos(c5_eq);
00467 driveValues_rob[ 4 ]=-sqrt((1.-c5)*(7.+c5))*driveValues[4]/2./s5;
00468 }else{
00469 if (positionValues[4]<KINEMATICS_EPS){
00470 alpha = M_PI_2;
00471 positionValues_kin[4] = 0.0;
00472 driveValues_rob[4]=driveValues[4];
00473 }else{
00474 alpha = atan2( s5, SQRT3d2 * ( 1. - c5 ) );
00475 positionValues_kin[4] = 2.*acos(c5_eq);
00476 driveValues_rob[4] = sqrt((1.-c5)*(7.+c5))*driveValues[4]/2./s5;
00477 }
00478 }
00479
00480 positionValues_kin[ 3 ] = positionValues[ 3 ] + alpha;
00481 positionValues_kin[ 5 ] = positionValues[ 5 ] - alpha;
00482
00483 double alphadot = -SQRT3t2/(7.+c5)*driveValues[4];
00484
00485 driveValues_rob[3] = driveValues[3]-alphadot;
00486 driveValues_rob[5] = driveValues[5]+alphadot;
00487
00488 driveValues.swap(driveValues_rob);
00489 positionValues.swap(positionValues_kin);
00490
00491 }
00492
00493
00494 }
00495 ORO_CREATE_COMPONENT_TYPE()
00496 ORO_LIST_COMPONENT_TYPE( OCL::Kuka361nAxesVelocityController )
