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 "Kuka361nAxesTorqueController.hpp"
00028 #include <ocl/ComponentLoader.hpp>
00029 #include <rtt/Logger.hpp>
00030
00031 ORO_LIST_COMPONENT_TYPE( OCL::Kuka361nAxesTorqueController )
00032
00033 namespace OCL
00034 {
00035 using namespace RTT;
00036 using namespace std;
00037
00038 #define KUKA361_NUM_AXES 6
00039
00040 #define KUKA361_ENCODEROFFSETS { 1000004, 1000000, 502652, 1001598, 985928, 1230656 }
00041
00042
00043 #define KUKA361_CONV1 94.14706
00044 #define KUKA361_CONV2 -103.23529
00045 #define KUKA361_CONV3 51.44118
00046 #define KUKA361_CONV4 175
00047 #define KUKA361_CONV5 150
00048 #define KUKA361_CONV6 131.64395
00049 #define KUKA361_ENC_RES 4096
00050
00051
00052 #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)}
00053
00054
00055 #define KUKA361_RADproSEC2VOLT { 2.5545, 2.67804024532652, 1.37350318088664, 2.34300679603342, 2.0058, 1.7573 } //18 april 2006
00056
00057
00058
00059
00060 #define KUKA361_TACHOSCALE { 0, 0, 0, 0, 0, 0 }
00061 #define KUKA361_TACHOOFFSET { 0, 0, 0, 0, 0, 0 }
00062
00063
00064
00065 #define KUKA361_KM { 0.2781*94.14706, 0.2863*103.23529, 0.2887*51.44118, 0.07*175, 0.07*150, 0.07*131.64395 }
00066
00067
00068
00069 #define KUKA361_A { 0.9030, 0.9091, 0.8929, 0.8909, 0.8962, 0.4716*1.92260 }
00070 #define KUKA361_B { 0.0896, 0.1072, 0.0867, 0.117, 0.0822, 0.0472*1.92260 }
00071 #define KUKA361_R { 0.1756, 0.1742, 0.1745, 0.1753, 0.1792, 0.1785 }
00072
00073 #define KUKA361_C { 0.7109, 0.3251, 0.0566, 0.1016, 0.0950, -1.0518 }
00074
00075
00076
00077 #define TACHO_OFFSET 0
00078 #define CURRENT_OFFSET KUKA361_NUM_AXES
00079 #define MODE_OFFSET 0
00080
00081
00082 typedef Kuka361nAxesTorqueController MyType;
00083
00084 Kuka361nAxesTorqueController::Kuka361nAxesTorqueController(string name)
00085 : TaskContext(name,PreOperational),
00086 driveValues("nAxesOutputValue",vector<double>(KUKA361_NUM_AXES)),
00087 driveValues_local(KUKA361_NUM_AXES),
00088 positionValues("nAxesSensorPosition",vector<double>(KUKA361_NUM_AXES)),
00089 positionValues_local(KUKA361_NUM_AXES),
00090 velocityValues("nAxesSensorVelocity",vector<double>(KUKA361_NUM_AXES)),
00091 velocityValues_local(KUKA361_NUM_AXES),
00092 torqueValues("nAxesSensorTorque",vector<double>(KUKA361_NUM_AXES)),
00093 torqueValues_local(KUKA361_NUM_AXES),
00094 delta_time_Port("_delta_time_Port"),
00095 velocityLimits("velocityLimits","velocity limits of the axes, (rad/s)",vector<double>(KUKA361_NUM_AXES,0)),
00096 currentLimits("currentLimits","current limits of the axes, (A)",vector<double>(KUKA361_NUM_AXES,0)),
00097 lowerPositionLimits("LowerPositionLimits","Lower position limits (rad)",vector<double>(KUKA361_NUM_AXES,0)),
00098 upperPositionLimits("UpperPositionLimits","Upper position limits (rad)",vector<double>(KUKA361_NUM_AXES,0)),
00099 initialPosition("initialPosition","Initial position (rad) for simulation or hardware",vector<double>(KUKA361_NUM_AXES,0)),
00100 velDriveOffset("velDriveOffset","offset (in rad/s) to the drive value.",vector<double>(KUKA361_NUM_AXES,0)),
00101 simulation("simulation","true if simulationAxes should be used",true),
00102 num_axes("NUM_AXES",KUKA361_NUM_AXES),
00103 velocityOutOfRange("velocityOutOfRange"),
00104 currentOutOfRange("currentOutOfRange"),
00105 positionOutOfRange("positionOutOfRange"),
00106 velresolved("velresolved","true if robot should be velocityresolved",false),
00107 TorqueMode(false),
00108 activated(false),
00109 positionConvertFactor(KUKA361_NUM_AXES),
00110 driveConvertFactor(KUKA361_NUM_AXES),
00111 tachoConvertScale(KUKA361_NUM_AXES),
00112 tachoConvertOffset(KUKA361_NUM_AXES),
00113 curReg_a(KUKA361_NUM_AXES),
00114 curReg_b(KUKA361_NUM_AXES),
00115 shunt_R(KUKA361_NUM_AXES),
00116 shunt_c(KUKA361_NUM_AXES),
00117 Km(KUKA361_NUM_AXES),
00118 EmergencyEvents_prop("EmergencyEvents","List of events that will result in an emergencystop of the robot"),
00119 #if (defined (OROPKG_OS_LXRT))
00120 axes_hardware(KUKA361_NUM_AXES),
00121 encoderInterface(KUKA361_NUM_AXES),
00122 encoder(KUKA361_NUM_AXES),
00123 ref(KUKA361_NUM_AXES),
00124 enable(KUKA361_NUM_AXES),
00125 drive(KUKA361_NUM_AXES),
00126 brake(KUKA361_NUM_AXES),
00127 tachoInput(KUKA361_NUM_AXES),
00128 tachometer(KUKA361_NUM_AXES),
00129 currentInput(KUKA361_NUM_AXES),
00130 currentSensor(KUKA361_NUM_AXES),
00131 torqueModeCheck(KUKA361_NUM_AXES),
00132 #endif
00133 axes(KUKA361_NUM_AXES),
00134 axes_simulation(KUKA361_NUM_AXES),
00135 tau_sim(KUKA361_NUM_AXES,0.0),
00136 pos_sim(KUKA361_NUM_AXES),
00137 previous_time(KUKA361_NUM_AXES)
00138 {
00139 double ticks2rad[KUKA361_NUM_AXES] = KUKA361_TICKS2RAD;
00140 double vel2volt[KUKA361_NUM_AXES] = KUKA361_RADproSEC2VOLT;
00141 double tachoscale[KUKA361_NUM_AXES] = KUKA361_TACHOSCALE;
00142 double tachooffset[KUKA361_NUM_AXES] = KUKA361_TACHOOFFSET;
00143 double CurReg_a[KUKA361_NUM_AXES] = KUKA361_A;
00144 double CurReg_b[KUKA361_NUM_AXES] = KUKA361_B;
00145 double Shunt_R[KUKA361_NUM_AXES] = KUKA361_R;
00146 double Shunt_c[KUKA361_NUM_AXES] = KUKA361_C;
00147 double KM[KUKA361_NUM_AXES] = KUKA361_KM;
00148 for(unsigned int i = 0;i<KUKA361_NUM_AXES;i++){
00149 positionConvertFactor[i] = ticks2rad[i];
00150 driveConvertFactor[i] = vel2volt[i];
00151 tachoConvertScale[i] = tachoscale[i];
00152 tachoConvertOffset[i] = tachooffset[i];
00153 curReg_a[i] = CurReg_a[i];
00154 curReg_b[i] = CurReg_b[i];
00155 shunt_R[i] = Shunt_R[i];
00156 shunt_c[i] = Shunt_c[i];
00157 Km[i] = KM[i];
00158 }
00159
00160
00161 properties()->addProperty( &velocityLimits );
00162 properties()->addProperty( ¤tLimits );
00163 properties()->addProperty( &lowerPositionLimits );
00164 properties()->addProperty( &upperPositionLimits );
00165 properties()->addProperty( &initialPosition );
00166 properties()->addProperty( &velDriveOffset );
00167 properties()->addProperty( &simulation );
00168 properties()->addProperty( &velresolved);
00169 properties()->addProperty( &EmergencyEvents_prop);
00170 attributes()->addConstant( &num_axes);
00171
00172
00173 this->methods()->addMethod( method("startAxes", &MyType::startAllAxes, this),
00174 "start all axes, starts updating the drive-value (only possible after unlockAxes)");
00175 this->methods()->addMethod( method("stopAxes", &MyType::stopAllAxes, this),
00176 "stop all axes, sets drive value to zero and disables the update of the drive-port, (only possible if axis is started)");
00177 this->methods()->addMethod( method("lockAxes", &MyType::lockAllAxes, this),"lock all axes, enables the brakes (only possible if axis is stopped");
00178 this->methods()->addMethod( method( "unlockAxes", &MyType::unlockAllAxes, this),
00179 "unlock all axis, disables the brakes and enables the drives (only possible if axis is locked");
00180 this->commands()->addCommand(command("prepareForUse", &MyType::prepareForUse,&MyType::prepareForUseCompleted, this),
00181 "prepares the robot for use" );
00182 this->commands()->addCommand(command( "prepareForShutdown", &MyType::prepareForShutdown,&MyType::prepareForShutdownCompleted, this),
00183 "prepares the robot for shutdown" );
00184 this->methods()->addMethod( method("addDriveOffset", &MyType::addDriveOffset, this),
00185 "adds offsets to the drive value of the axes","offsets","offset values in rad/s" );
00186
00190 this->ports()->addPort(&driveValues);
00191 this->ports()->addPort(&positionValues);
00192 this->ports()->addPort(&velocityValues);
00193 this->ports()->addPort(&torqueValues);
00194 this->ports()->addPort(&delta_time_Port);
00195
00199 events()->addEvent( &velocityOutOfRange, "Velocity of an Axis is out of range","message","Information about event" );
00200 events()->addEvent( &positionOutOfRange, "Position of an Axis is out of range","message","Information about event");
00201 events()->addEvent( ¤tOutOfRange, "Current of an Axis is out of range","message","Information about event");
00202
00203
00204 }
00205
00206 bool Kuka361nAxesTorqueController::configureHook()
00207 {
00208
00209 TorqueMode = !velresolved.rvalue();
00210
00211
00212 #if (defined (OROPKG_OS_LXRT))
00213 int encoderOffsets[KUKA361_NUM_AXES] = KUKA361_ENCODEROFFSETS;
00214
00215 comediDev = new ComediDevice( 1 );
00216 comediSubdevAOut = new ComediSubDeviceAOut( comediDev, "NI6713_AO" );
00217 apci1710 = new EncoderSSI_apci1710_board( 0, 1, 2 );
00218 apci2200 = new RelayCardapci2200( "APCI2200" );
00219 apci1032 = new SwitchDigitalInapci1032( "APCI1032" );
00220 comediDev_NI6024 = new ComediDevice( 4 );
00221 comediSubdevAIn_NI6024 = new ComediSubDeviceAIn( comediDev_NI6024, "NI6024_AI", 0 );
00222 comediSubdevDIn_NI6024 = new ComediSubDeviceDIn( comediDev_NI6024, "NI6024_DI", 2 );
00223 comediSubdevDOut_NI6713 = new ComediSubDeviceDOut( comediDev, "NI6713_AO", 2 );
00224
00225 torqueModeSwitch = new DigitalOutput(comediSubdevDOut_NI6713,0);
00226 if(TorqueMode)
00227 torqueModeSwitch->switchOn();
00228 else
00229 torqueModeSwitch->switchOff();
00230
00231
00232
00233 encoderInterface[0] = new EncoderSSI_apci1710( 1, apci1710 );
00234 encoderInterface[1] = new EncoderSSI_apci1710( 2, apci1710 );
00235 encoderInterface[2] = new EncoderSSI_apci1710( 7, apci1710 );
00236 encoderInterface[3] = new EncoderSSI_apci1710( 8, apci1710 );
00237 encoderInterface[4] = new EncoderSSI_apci1710( 5, apci1710 );
00238 encoderInterface[5] = new EncoderSSI_apci1710( 6, apci1710 );
00239
00240 for (unsigned int i = 0; i < KUKA361_NUM_AXES; i++){
00241
00242 encoder[i] = new AbsoluteEncoderSensor( encoderInterface[i], 1.0 / positionConvertFactor[i], encoderOffsets[i], -10, 10 );
00243
00244 brake[i] = new DigitalOutput( apci2200, i + KUKA361_NUM_AXES );
00245 log(Info)<<"Setting brake "<<i<<" On."<<endlog();
00246 brake[i]->switchOn();
00247
00248 tachoInput[i] = new AnalogInput(comediSubdevAIn_NI6024, i+TACHO_OFFSET);
00249 unsigned int range = 0;
00250 comediSubdevAIn_NI6024->rangeSet(i+TACHO_OFFSET, range);
00251 comediSubdevAIn_NI6024->arefSet(i+TACHO_OFFSET, AnalogInInterface::Common);
00252 tachometer[i] = new AnalogSensor( tachoInput[i], comediSubdevAIn_NI6024->lowest(i+TACHO_OFFSET), comediSubdevAIn_NI6024->highest(i+TACHO_OFFSET), tachoConvertScale[i], tachoConvertOffset[i]);
00253
00254 currentInput[i] = new AnalogInput(comediSubdevAIn_NI6024, i+CURRENT_OFFSET);
00255 range = 1;
00256 comediSubdevAIn_NI6024->rangeSet(i+CURRENT_OFFSET, range);
00257 comediSubdevAIn_NI6024->arefSet(i+CURRENT_OFFSET, AnalogInInterface::Common);
00258 currentSensor[i] = new AnalogSensor( currentInput[i], comediSubdevAIn_NI6024->lowest(i+CURRENT_OFFSET), comediSubdevAIn_NI6024->highest(i+CURRENT_OFFSET), 1.0 , 0);
00259
00260 torqueModeCheck[i] = new DigitalInput( comediSubdevDIn_NI6024, i );
00261
00262 ref[i] = new AnalogOutput( comediSubdevAOut, i );
00263 enable[i] = new DigitalOutput( apci2200, i );
00264
00265
00266 if ( TorqueMode ){
00267 if ( !torqueModeCheck[i]->isOn() ) {
00268 log(Error) << "Failed to switch relay of channel " << i << " to torque control mode" << endlog();
00269 return false;
00270 }
00271 drive[i] = new AnalogDrive( ref[i], enable[i], curReg_a[i] / shunt_R[i], - shunt_c[i] - (curReg_b[i] / shunt_R[i]));
00272
00273 }else{
00274 drive[i] = new AnalogDrive( ref[i], enable[i], 1.0 / driveConvertFactor[i], velDriveOffset.value()[i]);
00275 }
00276
00277 axes_hardware[i] = new Axis( drive[i] );
00278 axes_hardware[i]->setBrake( brake[i] );
00279 axes_hardware[i]->setSensor( "Position", encoder[i] );
00280 axes_hardware[i]->setSensor( "Velocity", tachometer[i] );
00281 axes_hardware[i]->setSensor( "Current", currentSensor[i] );
00282 axes_hardware[i]->setSwitch( "Mode", torqueModeCheck[i] );
00283
00284 if ( TorqueMode ){
00285 axes_hardware[i]->limitDrive(-currentLimits.value()[i], currentLimits.value()[i], currentOutOfRange);
00286 }else{
00287 axes_hardware[i]->limitDrive(-velocityLimits.value()[i], velocityLimits.value()[i], velocityOutOfRange);
00288 }
00289 }
00290
00291
00292 #endif
00293 for (unsigned int i = 0; i <KUKA361_NUM_AXES; i++){
00294 if ( TorqueMode ) {
00295 axes_simulation[i] = new OCL::TorqueSimulationAxis(initialPosition.value()[i], lowerPositionLimits.value()[i], upperPositionLimits.value()[i],velocityLimits.value()[i]);
00296 } else {
00297 axes_simulation[i] = new OCL::SimulationAxis(initialPosition.value()[i], lowerPositionLimits.value()[i], upperPositionLimits.value()[i]);
00298 }
00299 }
00300
00301 for(unsigned int i = 0;i<KUKA361_NUM_AXES;i++){
00302 pos_sim[i] = initialPosition.value()[i];
00303 }
00304 torqueSimulator = new Kuka361TorqueSimulator(axes_simulation, pos_sim);
00305
00306 #if (defined (OROPKG_OS_LXRT))
00307 if(!simulation.rvalue()){
00308 for (unsigned int i = 0; i <KUKA361_NUM_AXES; i++)
00309 axes[i] = axes_hardware[i];
00310 log(Info) << "LXRT version of Kuka361nAxesTorqueController has started" << endlog();
00311 }
00312 else{
00313 for (unsigned int i = 0; i <KUKA361_NUM_AXES; i++)
00314 axes[i] = axes_simulation[i];
00315 log(Info) << "LXRT simulation version of Kuka361nAxesTorqueController has started" << endlog();
00316 }
00317 #else
00318 for (unsigned int i = 0; i <KUKA361_NUM_AXES; i++)
00319 axes[i] = axes_simulation[i];
00320 log(Info) << "GNULINUX simulation version of Kuka361nAxesTorqueController has started" << endlog();
00321 #endif
00322 if(EmergencyEvents_prop.ready())
00323 for(unsigned int i=0;i<EmergencyEvents_prop.value().size();i++){
00324 string name = EmergencyEvents_prop.value()[i];
00325 string::size_type idx = name.find('.');
00326 if(idx==string::npos)
00327 log(Warning)<<"Could not connect EmergencyStop to "<<name<<"\n Syntax of "
00328 <<name<<" is not correct. I want a ComponentName.EventName "<<endlog();
00329 string peername = name.substr(0,idx);
00330 string eventname = name.substr(idx+1);
00331 TaskContext* peer;
00332 if(peername==this->getName())
00333 peer = this;
00334 else if(this->hasPeer(peername)){
00335 peer = this->getPeer(peername);
00336 }else{
00337 log(Warning)<<"Could not connect EmergencyStop to "<<name<<", "<<peername<<" is not a peer of "<<this->getName()<<endlog();
00338 continue;
00339 }
00340
00341 if(peer->events()->hasEvent(eventname)){
00342 Handle handle = peer->events()->setupConnection(eventname).callback(this,&Kuka361nAxesTorqueController::EmergencyStop).handle();
00343 if(handle.connect()){
00344 EmergencyEventHandlers.push_back(handle);
00345 log(Info)<<"EmergencyStop connected to "<< name<<" event."<<endlog();
00346 }else
00347 log(Warning)<<"Could not connect EmergencyStop to "<<name<<", "<<eventname<<" has to have a message parameter."<<endlog();
00348 }else
00349 log(Warning)<<"Could not connect EmergencyStop to "<<name<<", "<<eventname <<" not found in "<<peername<<"s event-list"<<endlog();
00350 }else{
00351 log(Warning)<<"No EmergencyStops available"<<endlog();
00352 }
00353
00354 return true;
00355 }
00356
00357
00358 Kuka361nAxesTorqueController::~Kuka361nAxesTorqueController()
00359 {
00360 }
00361
00362 void Kuka361nAxesTorqueController::cleanupHook()
00363 {
00364
00365 prepareForShutdown();
00366
00367
00368 for (unsigned int i = 0; i < KUKA361_NUM_AXES; i++)
00369 delete axes_simulation[i];
00370
00371 delete torqueSimulator;
00372
00373 #if (defined (OROPKG_OS_LXRT))
00374 for (unsigned int i = 0; i < KUKA361_NUM_AXES; i++)
00375 delete axes_hardware[i];
00376 delete comediDev;
00377 delete comediSubdevAOut;
00378 delete apci1710;
00379 delete apci2200;
00380 delete apci1032;
00381 delete comediDev_NI6024;
00382 delete comediSubdevAIn_NI6024;
00383 delete comediSubdevDIn_NI6024;
00384
00385 #endif
00386 }
00387
00388
00389 bool Kuka361nAxesTorqueController::startHook()
00390 {
00391 for (int axis=0;axis<KUKA361_NUM_AXES;axis++) {
00392 previous_time[axis] = TimeService::Instance()->getTicks();
00393 positionValues_local[axis] = axes[axis]->getSensor("Position")->readSensor();
00394 }
00395 positionValues.Set(positionValues_local);
00396
00397 return true;
00398 }
00399
00400 void Kuka361nAxesTorqueController::updateHook()
00401 {
00402 driveValues_local = driveValues.Get();
00403 positionValues_local = positionValues.Get();
00404 #if (defined (OROPKG_OS_LXRT))
00405 if(simulation.rvalue()) {
00406
00407
00408 torqueSimulator->update(driveValues_local);
00409 }
00410 #else
00411
00412 torqueSimulator->update(driveValues_local);
00413 #endif
00414
00415 for (int axis=0;axis<KUKA361_NUM_AXES;axis++) {
00416
00417 delta_time = TimeService::Instance()->secondsSince(previous_time[axis]);
00418 velocityValues_local[axis] = (axes[axis]->getSensor("Position")->readSensor() - positionValues_local[axis])/delta_time;
00419 previous_time[axis] = TimeService::Instance()->getTicks();
00420 positionValues_local[axis] = axes[axis]->getSensor("Position")->readSensor();
00421 delta_time_Port.Set(delta_time);
00422
00423
00424 if( (velocityValues_local[axis] < -velocityLimits.value()[axis]) ||
00425 (velocityValues_local[axis] > velocityLimits.value()[axis]) ){
00426 char msg[80];
00427 sprintf(msg,"Velocity of Kuka361 Axis %d is out of range: %f",axis+1,velocityValues_local[axis]);
00428 velocityOutOfRange(msg);
00429 }
00430
00431
00432 if( (positionValues_local[axis] < lowerPositionLimits.value()[axis]) ||
00433 (positionValues_local[axis] > upperPositionLimits.value()[axis]) ){
00434 char msg[80];
00435 sprintf(msg,"Position of Kuka361 Axis %d is out of range: %f",axis+1,positionValues_local[axis]);
00436 positionOutOfRange(msg);
00437 }
00438
00439
00440 if (axes[axis]->isDriven()) {
00441 if( TorqueMode ){
00442 axes[axis]->drive(driveValues_local[axis] / Km[axis]);
00443 }
00444 else
00445 axes[axis]->drive(driveValues_local[axis]);
00446 }
00447
00448 #if (defined (OROPKG_OS_LXRT))
00449 if(!simulation.rvalue()) {
00450
00451 if( TorqueMode){
00452 torqueValues_local[axis] = (axes[axis]->getSensor("Current")->readSensor() / shunt_R[axis] + shunt_c[axis]) * Km[axis];
00453 }
00454 }
00455 #endif
00456 }
00457 torqueValues.Set(torqueValues_local);
00458 velocityValues.Set(velocityValues_local);
00459 positionValues.Set(positionValues_local);
00460 }
00461
00462
00463
00464 void Kuka361nAxesTorqueController::stopHook()
00465 {
00466
00467 prepareForShutdown();
00468
00469
00470 }
00471
00472
00473 bool Kuka361nAxesTorqueController::prepareForUse()
00474 {
00475 #if (defined (OROPKG_OS_LXRT))
00476 if(!simulation.rvalue()){
00477 apci2200->switchOn( 12 );
00478 apci2200->switchOn( 14 );
00479 log(Warning) <<"Release Emergency stop and push button to start ..."<<endlog();
00480 }
00481 #endif
00482 activated = true;
00483 return true;
00484 }
00485
00486 bool Kuka361nAxesTorqueController::prepareForUseCompleted()const
00487 {
00488 #if (defined (OROPKG_OS_LXRT))
00489 if(!simulation.rvalue())
00490 return (apci1032->isOn(12) && apci1032->isOn(14));
00491 else
00492 #endif
00493 return true;
00494 }
00495
00496 bool Kuka361nAxesTorqueController::prepareForShutdown()
00497 {
00498
00499 stopAllAxes();
00500 lockAllAxes();
00501 #if (defined (OROPKG_OS_LXRT))
00502 torqueModeSwitch->switchOff();
00503
00504 if(!simulation.rvalue()){
00505 apci2200->switchOff( 12 );
00506 apci2200->switchOff( 14 );
00507 }
00508
00509 #endif
00510 activated = false;
00511 return true;
00512 }
00513
00514 bool Kuka361nAxesTorqueController::prepareForShutdownCompleted()const
00515 {
00516 return true;
00517 }
00518
00519 bool Kuka361nAxesTorqueController::stopAllAxes()
00520 {
00521 bool _return = true;
00522 for(unsigned int i = 0;i<KUKA361_NUM_AXES;i++){
00523 _return &= axes[i]->stop();
00524 }
00525 return _return;
00526 }
00527
00528 bool Kuka361nAxesTorqueController::startAllAxes()
00529 {
00530 bool _return = true;
00531 for(unsigned int i = 0;i<KUKA361_NUM_AXES;i++){
00532 _return &= axes[i]->drive(0.0);
00533 }
00534 return _return;
00535 }
00536
00537 bool Kuka361nAxesTorqueController::unlockAllAxes()
00538 {
00539 bool _return = true;
00540 for(unsigned int i = 0;i<KUKA361_NUM_AXES;i++){
00541 _return &= axes[i]->unlock();
00542 }
00543 return _return;
00544 }
00545
00546 bool Kuka361nAxesTorqueController::lockAllAxes()
00547 {
00548 bool _return = true;
00549 for(unsigned int i = 0;i<KUKA361_NUM_AXES;i++){
00550 _return &= axes[i]->lock();
00551 }
00552 return _return;
00553 }
00554
00555
00556 bool Kuka361nAxesTorqueController::addDriveOffset(const vector<double>& offset)
00557 {
00558 for(unsigned int i=0;i<KUKA361_NUM_AXES;i++){
00559 velDriveOffset.value()[i] += offset[i];
00560
00561 #if (defined (OROPKG_OS_LXRT))
00562 if (!simulation.rvalue())
00563 ((Axis*)(axes[i]))->getDrive()->addOffset(offset[i]);
00564 #endif
00565 }
00566 return true;
00567 }
00568
00569 }
00570
