Libbarrett  1.2.4
include/barrett/systems/detail/low_level_wam_wrapper-inl.h
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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Defines