Libbarrett
1.2.4
|
00001 /* 00002 Copyright 2009, 2010 Barrett Technology <support@barrett.com> 00003 00004 This file is part of libbarrett. 00005 00006 This version of libbarrett is free software: you can redistribute it 00007 and/or modify it under the terms of the GNU General Public License as 00008 published by the Free Software Foundation, either version 3 of the 00009 License, or (at your option) any later version. 00010 00011 This version of libbarrett is distributed in the hope that it will be 00012 useful, but WITHOUT ANY WARRANTY; without even the implied warranty of 00013 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00014 GNU General Public License for more details. 00015 00016 You should have received a copy of the GNU General Public License along 00017 with this version of libbarrett. If not, see 00018 <http://www.gnu.org/licenses/>. 00019 00020 Further, non-binding information about licensing is available at: 00021 <http://wiki.barrett.com/libbarrett/wiki/LicenseNotes> 00022 */ 00023 00024 /* 00025 * low_level_wam_wrapper-inl.h 00026 * 00027 * Created on: Feb 2, 2010 00028 * Author: dc 00029 */ 00030 00031 00032 #include <vector> 00033 #include <stdexcept> 00034 00035 #include <libconfig.h++> 00036 00037 #include <barrett/products/puck.h> 00038 #include <barrett/products/low_level_wam.h> 00039 #include <barrett/products/safety_module.h> 00040 00041 00042 namespace barrett { 00043 namespace systems { 00044 00045 00046 template<size_t DOF> 00047 LowLevelWamWrapper<DOF>::LowLevelWamWrapper( 00048 ExecutionManager* em, 00049 const std::vector<Puck*>& genericPucks, 00050 SafetyModule* safetyModule, 00051 const libconfig::Setting& setting, 00052 std::vector<int> torqueGroupIds, 00053 const std::string& sysName) : 00054 input(sink.input), 00055 jpOutput(source.jpOutput), jvOutput(source.jvOutput), 00056 llw(genericPucks, safetyModule, setting, torqueGroupIds), 00057 sink(this, em, sysName + "::Sink"), source(this, em, sysName + "::Source") 00058 { 00059 } 00060 00061 template<size_t DOF> 00062 void LowLevelWamWrapper<DOF>::Sink::operate() 00063 { 00064 parent->llw.setTorques(this->input.getValue()); 00065 } 00066 00067 template<size_t DOF> 00068 void LowLevelWamWrapper<DOF>::Source::operate() 00069 { 00070 try { 00071 parent->llw.update(); 00072 } catch (const std::runtime_error& e) { 00073 if (parent->llw.getSafetyModule() != NULL && parent->llw.getSafetyModule()->getMode(true) == SafetyModule::ESTOP) { 00074 throw ExecutionManagerException("systems::LowLevelWamWrapper::Source::operate(): E-stop! Cannot communicate with Pucks."); 00075 } else { 00076 throw; 00077 } 00078 } 00079 00080 this->jpOutputValue->setData( &(parent->llw.getJointPositions()) ); 00081 this->jvOutputValue->setData( &(parent->llw.getJointVelocities()) ); 00082 } 00083 00084 00085 } 00086 }