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
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
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
00063 m_in_Angles("in_Angles"),
00064 m_in_Velocities("in_Velocities"),
00065 m_in_Currents("in_Currents"),
00066 m_out_Angles("out_Angles"),
00067 m_out_Velocities("out_Velocities"),
00068
00069 m_stop_method("stop_movement",&PowerCubeCtrl_OROCOS::stopArm, this),
00070
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
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
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
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
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
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
00207 stopArm();
00208 }
00209
00210 bool PowerCubeCtrl_OROCOS::startHook()
00211 {
00212 if ( m_in_Angles.connected() )
00213 {
00214
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
00222 return true;
00223 }
00224 if ( m_in_Velocities.connected() )
00225 {
00226
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
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
00243
00244
00245
00246
00247
00248
00249
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
00262
00263
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
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
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 }