PowerCubeSim_OROCOS.cpp
Go to the documentation of this file.
00001 /****************************************************************
00002  *
00003  * Copyright (c) 2010
00004  *
00005  * Fraunhofer Institute for Manufacturing Engineering   
00006  * and Automation (IPA)
00007  *
00008  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00009  *
00010  * Project name: care-o-bot
00011  * ROS stack name: cob_driver
00012  * ROS package name: cob_powercube_chain
00013  * Description:
00014  *                                                              
00015  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00016  *                      
00017  * Author: Alexander Bubeck, email:alexander.bubeck@ipa.fhg.de
00018  * Supervised by: Alexander Bubeck, email:alexander.bubeck@ipa.fhg.de
00019  *
00020  * Date of creation: Dec 2009
00021  * ToDo:
00022  *
00023  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00024  *
00025  * Redistribution and use in source and binary forms, with or without
00026  * modification, are permitted provided that the following conditions are met:
00027  *
00028  *     * Redistributions of source code must retain the above copyright
00029  *       notice, this list of conditions and the following disclaimer.
00030  *     * Redistributions in binary form must reproduce the above copyright
00031  *       notice, this list of conditions and the following disclaimer in the
00032  *       documentation and/or other materials provided with the distribution.
00033  *     * Neither the name of the Fraunhofer Institute for Manufacturing 
00034  *       Engineering and Automation (IPA) nor the names of its
00035  *       contributors may be used to endorse or promote products derived from
00036  *       this software without specific prior written permission.
00037  *
00038  * This program is free software: you can redistribute it and/or modify
00039  * it under the terms of the GNU Lesser General Public License LGPL as 
00040  * published by the Free Software Foundation, either version 3 of the 
00041  * License, or (at your option) any later version.
00042  * 
00043  * This program is distributed in the hope that it will be useful,
00044  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00045  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00046  * GNU Lesser General Public License LGPL for more details.
00047  * 
00048  * You should have received a copy of the GNU Lesser General Public 
00049  * License LGPL along with this program. 
00050  * If not, see <http://www.gnu.org/licenses/>.
00051  *
00052  ****************************************************************/
00053 
00054 #include "PowerCubeSim_OROCOS.h"
00055 #include "simulatedArm.h"
00056 #include <vector>
00057 
00058 using namespace RTT;
00059 
00060 PowerCubeSim_OROCOS::PowerCubeSim_OROCOS(std::string name) : OrocosRTTArmDriverInterface(name)
00061 {
00062 
00063 }
00064 
00065 PowerCubeSim_OROCOS::~PowerCubeSim_OROCOS()
00066 {
00067 }
00068 
00069 bool PowerCubeSim_OROCOS::configureHook()
00070 {
00071         if ( m_powercubectrl.Init(false) )
00072         {
00073                 log(Info) << "PowerCubeSim initialized successfully." << endlog();
00074                 return true;
00075         }
00076         else
00077         {
00078                 log(Info) << "Error while initializing PowerCubeSim:" << endlog();
00079                 //log(Info) << m_powercubectrl.getErrorMessage() <<  endlog();
00080                 return false;
00081         }
00082 }
00083 
00084 bool PowerCubeSim_OROCOS::startHook()
00085 {
00086  /*   if ( m_in_Angles.connected() )
00087     {
00088         // only one of the input ports should be used simultaniously
00089         if ( m_in_Velocities.connected() || m_in_Currents.connected() ) {
00090                 log(Info) << "Error in PowerCubeSim.startHook(): more than one input port is connected!" << endlog();
00091                 return false;
00092         }
00093         m_in_Angles_connected = true;
00094         log(Info) << "PowerCubeSim succesfully detected connection at in_Angles" << endlog();
00095             return true;
00096     }
00097     if ( m_in_Velocities.connected() )
00098     {
00099         // only one of the input ports should be used simultaniously
00100         if ( m_in_Angles.connected() || m_in_Currents.connected() ) {
00101                 log(Info) << "Error in PowerCubeSim.startHook(): more than one input port is connected!" << endlog();
00102                 return false;
00103         }
00104         m_in_Velocities_connected = true;
00105         log(Info) << "PowerCubeSim succesfully detected connection at in_Velocities" << endlog();
00106             return true;
00107     }
00108     if ( m_in_Currents.connected() )
00109     {
00110         log(Info) << "Error, port \"setCur_R\" is connected to PowerCubeSim_OROCOS.\"";
00111         log(Info) << "Current movement is not yet supported by PowerCubeSim." << endlog();
00112         return false;
00113         /*
00114         // only one of the input ports should be used simultaniously
00115         if ( m_in_Angles.connected() || m_in_Velocities.connected() ) {
00116                 log(Info) << "Error in PowerCubeSim.startHook(): more than one input port is connected!" << endlog();                   
00117                 return false;
00118         }
00119         m_in_Current_connected = true;
00120             return true;
00121 
00122     }
00123     else
00124     {
00125         log(Info) << "No Input port is connected, PowerCubeSim will only return the current state." << endlog();
00126     }    */
00127     return true;
00128 }
00129 
00130 void PowerCubeSim_OROCOS::updateHook()
00131 {
00132         //log(Info) << "updateHook is being executed." << endlog();
00133         Jointd setpositions, setvelocities;
00134         setpositions = set_position_inport.Get();
00135         setvelocities = set_velocity_inport.Get();
00136         if(setpositions.size() == 7)
00137         {
00138         
00139         }
00140         else if(setvelocities.size() == 7)
00141         {
00142 
00143         }
00144         
00145         current_position_outport.Set(m_powercubectrl.getConfig());
00146         current_velocity_outport.Set(m_powercubectrl.getJointVelocities());
00147 }
00148 
00149 void PowerCubeSim_OROCOS::stopHook()
00150 {
00151         stopArm();
00152 }
00153 
00154 bool PowerCubeSim_OROCOS::stopArm()
00155 {
00156     //stop
00157     m_powercubectrl.stop();
00158     return true;
00159 }
00160 
00161 bool PowerCubeSim_OROCOS::isArmStopped()
00162 {
00163     //isStopped
00164     if(m_powercubectrl.statusMoving())
00165         return false;
00166     return true;
00167 }


schunk_powercube_chain
Author(s): Florian Weisshardt
autogenerated on Thu Aug 27 2015 15:06:58