PowerCubeCtrl_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 "PowerCubeCtrl_OROCOS.h"
00055 
00056 using namespace RTT;
00057 
00058 PowerCubeCtrl_OROCOS::PowerCubeCtrl_OROCOS(std::string name, std::string xmlFile, int dof)
00059 : TaskContext(name),
00060         m_xmlFile(xmlFile),
00061         m_dof(dof),
00062         // initialize dataports:
00063         m_in_Angles("in_Angles"),
00064         m_in_Velocities("in_Velocities"),
00065         m_in_Currents("in_Currents"),
00066         m_out_Angles("out_Angles"),  // note: initial value
00067         m_out_Velocities("out_Velocities"),  // note: initial value
00068         // initialize method
00069         m_stop_method("stop_movement",&PowerCubeCtrl_OROCOS::stopArm, this),
00070         //m_moveJointSpace_method("moveJointSpace", &PowerCubeCtrl_OROCOS::moveJointSpace, this),
00071         m_CanDev_prop("CanDev_n", "Can Device No.", 0),
00072         m_CanBaud_prop("BaudRate", "Can Baud Rate", 1000),
00073         m_dof_prop("DOF", "Degrees of Freedom", 7),
00074         m_MaxVel_prop("Max_Vel", "Maximum Velocity", 0.5),
00075         m_MaxAcc_prop("Max_Acc", "Maximum Acceleration", 0.8),
00076         m_powercubectrl()
00077 {
00078     this->ports()->addPort( &m_in_Angles, "Port for desired Angles" );
00079     m_in_Angles_connected = false;
00080     this->ports()->addPort( &m_in_Velocities, "Port for desired velocities" );
00081     m_in_Velocities_connected = false;
00082     this->ports()->addPort( &m_in_Currents, "Port for desired currents" );
00083     m_in_Currents_connected = false;
00084 
00085     this->ports()->addPort( &m_out_Angles, "Port for current positions" );
00086     this->ports()->addPort( &m_out_Velocities, "Port for current velocities" );
00087 
00088     this->methods()->addMethod(&m_stop_method, "Stop the Arm");
00089     //this->methods()->addMethod(&m_moveJointSpace_method, "execute synchronized ramp commands");
00090     
00091         this->properties()->addProperty(&m_CanDev_prop);
00092         this->properties()->addProperty(&m_CanBaud_prop);
00093         this->properties()->addProperty(&m_dof_prop);
00094         this->properties()->addProperty(&m_MaxVel_prop);
00095         this->properties()->addProperty(&m_MaxAcc_prop);
00096 
00097         //m_mod_params.clear();
00098 
00099         m_modId_props.clear();
00100         m_ul_props.clear();
00101         m_ll_props.clear();
00102         m_offset_props.clear();
00103         
00104         for (int i=0; i<m_dof; i++)
00105         {
00106                 ostringstream os;
00107 
00108                 os << "modId" << i+1;
00109                 m_modId_props.push_back( Property<int>( os.str(), os.str(), 0 ) );
00110 
00111                 os.str("");     os.clear();
00112                 os << "upperLimit" << i+1;
00113                 m_ul_props.push_back( Property<double>( os.str(), os.str(), 0.0 ) );
00114 
00115                 os.str("");     os.clear();
00116                 os << "lowerLimit" << i+1;
00117                 m_ll_props.push_back( Property<double>( os.str(), os.str(), 0.0 ) );
00118                 
00119                 os.str("");     os.clear();
00120                 os << "offset" << i+1;
00121                 m_offset_props.push_back( Property<double>( os.str(), os.str(), 0.0 ) );
00122 
00123         }
00124 
00125         m_mod_params.clear();
00126         for (int i=0; i<m_dof; i++)
00127         {
00128                 ostringstream os;
00129                 os << "modParams" << i+1;
00130 
00131                 PropertyBag bag( os.str() );
00132                 bag.add( &m_modId_props[i] );
00133                 bag.add( &m_ul_props[i] );
00134                 bag.add( &m_ll_props[i] );
00135                 bag.add( &m_offset_props[i] );
00136 
00137                 Property<PropertyBag>  prop_of_bag( os.str(), "", bag);
00138 
00139                 m_mod_params.push_back( prop_of_bag );
00140         }
00141 
00142         for (int i=0; i<m_dof; i++)
00143         {
00144                 this->properties()->addProperty( &(m_mod_params[i]) );
00145         }
00146     //m_stopped = false;
00147 }
00148 
00149 PowerCubeCtrl_OROCOS::~PowerCubeCtrl_OROCOS()
00150 {
00151         stop();
00152 }
00153 
00154 bool PowerCubeCtrl_OROCOS::configureHook()
00155 {
00156         this->marshalling()->readProperties(m_xmlFile);
00157 
00158         // COPY PROPERTIES INTO STRUCT POWERCUBEPARAMETERS      
00159 
00160         PowerCubeParameters pcparams;
00161         
00162         pcparams.modIds.resize(m_dof);
00163         pcparams.upperlimits.resize(m_dof);
00164         pcparams.lowerlimits.resize(m_dof);
00165         pcparams.offsets.resize(m_dof);
00166         
00167         pcparams.dof=m_dof_prop.get();  
00168         pcparams.CanDevice=m_CanDev_prop.get();
00169         pcparams.BaudRate=m_CanBaud_prop.get();
00170         pcparams.maxVel=m_MaxVel_prop.get();
00171         pcparams.maxAcc=m_MaxAcc_prop.get(); 
00172         
00173         int i;
00174         if(m_dof != pcparams.dof)
00175         {
00176                 log(Info) << "Degrees of freedom" << m_dof << "doesn't match with xml input DOF." << pcparams.dof << endlog();
00177             return false;
00178         }
00179         
00180         for (i=0 ; i< (m_dof) ; i++)
00181         {
00182                 pcparams.modIds[i] = m_modId_props[i].get();
00183                 pcparams.upperlimits[i] = m_ul_props[i].get();
00184                 pcparams.lowerlimits[i] = m_ll_props[i].get();
00185                 pcparams.offsets[i] = m_offset_props[i].get();
00186         }
00187 
00188 //      int id = m_modId_props[3].get();
00189         
00190         if ( m_powercubectrl.Init(pcparams) )
00191         {
00192                 log(Info) << "PowerCubeCtrl initialized successfully." << endlog();
00193                 return true;
00194         }
00195         else
00196         {
00197                 log(Info) << "Error while initializing PowerCubeCtrl:" << endlog();
00198                 log(Info) << m_powercubectrl.getErrorMessage() <<  endlog();
00199                 return false;
00200         }
00201         return true;
00202 }
00203 
00204 void PowerCubeCtrl_OROCOS::stopHook()
00205 {
00206         //m_stopped = true;
00207         stopArm();
00208 }
00209 
00210 bool PowerCubeCtrl_OROCOS::startHook()
00211 {
00212     if ( m_in_Angles.connected() )
00213     {
00214         // only one of the input ports should be used simultaniously
00215         if ( m_in_Velocities.connected() || m_in_Currents.connected() ) {
00216                 log(Info) << "Error in PowerCubeSim.startHook(): more than one input port is connected!" << endlog();
00217                 return false;
00218         }
00219         m_in_Angles_connected = true;
00220         log(Info) << "PowerCubeSim succesfully detected connection at in_Angles" << endlog();
00221         //m_stopped = false;
00222             return true;
00223     }
00224     if ( m_in_Velocities.connected() )
00225     {
00226         // only one of the input ports should be used simultaniously
00227         if ( m_in_Angles.connected() || m_in_Currents.connected() ) {
00228                 log(Info) << "Error in PowerCubeSim.startHook(): more than one input port is connected!" << endlog();
00229                 return false;
00230         }
00231         m_in_Velocities_connected = true;
00232         log(Info) << "PowerCubeSim succesfully detected connection at in_Velocities" << endlog();
00233         //m_stopped = false;
00234         return true;
00235     }
00236     if ( m_in_Currents.connected() )
00237     {
00238         log(Info) << "Error, port \"setCur_R\" is connected to PowerCubeSim_OROCOS.\"";
00239         log(Info) << "Current movement is not yet supported by PowerCubeSim." << endlog();
00240         return false;
00241         /*
00242         // only one of the input ports should be used simultaniously
00243         if ( m_in_Angles.connected() || m_in_Velocities.connected() ) {
00244                 log(Info) << "Error in PowerCubeSim.startHook(): more than one input port is connected!" << endlog();                   
00245                 return false;
00246         }
00247         m_in_Current_connected = true;
00248         //m_stopped = false;
00249             return true;
00250             */
00251     }
00252     else
00253     {
00254         log(Info) << "No Input port is connected, PowerCubeSim will only return the current state." << endlog();
00255     }
00256     return true;
00257 }
00258 
00259 void PowerCubeCtrl_OROCOS::updateHook()
00260 {
00261         // log(Info) << "updateHook is being executed." << endlog();
00262         
00263         // Execute desired movements:
00264         if ( m_in_Angles_connected )
00265         {
00266                 std::vector<double> angles_desired(7);
00267                 angles_desired = m_in_Angles.Get();
00268             m_powercubectrl.MovePos( angles_desired );
00269         }
00270         
00271         if ( m_in_Velocities_connected )
00272         {
00273                 std::vector<double> vel_desired(7);
00274                 vel_desired = m_in_Velocities.Get();
00275             m_powercubectrl.MoveVel( vel_desired );
00276         }
00277         if ( m_in_Currents_connected )
00278         {
00279                 std::vector<double> currents_desired(7);
00280                 currents_desired = m_in_Currents.Get();
00281             m_powercubectrl.MoveCur( currents_desired );
00282         }
00283 
00284     // get current angles & velocities
00285     std::vector<double> curConfig(7);
00286         m_powercubectrl.getConfig(curConfig);
00287     m_out_Angles.Set(curConfig);
00288 
00289     std::vector<double> curVelocities(7);
00290     m_powercubectrl.getJointVelocities(curVelocities);
00291     m_out_Velocities.Set(curVelocities);        
00292 }
00293 
00294 bool PowerCubeCtrl_OROCOS::stopArm()
00295 {
00296     //stop
00297     m_powercubectrl.Stop();             
00298     return true;
00299 }
00300 
00301 bool PowerCubeCtrl_OROCOS::moveJointSpace(std::vector<double> target)           
00302 {
00303         return m_powercubectrl.MoveJointSpaceSync(target);
00304 }
00305 
00306 bool PowerCubeCtrl_OROCOS::isArmStopped()
00307 {
00308         return !m_powercubectrl.statusMoving();
00309 }


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