00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00013
00014
00015 #include <sys/time.h>
00016 #include <math.h>
00017 #include <stdlib.h>
00018
00019
00020 #include "stage.hh"
00021 #include "worldfile.hh"
00022
00023 using namespace Stg;
00024
00059 static const double WATTS_KGMS = 5.0;
00060 static const double WATTS_BASE = 2.0;
00061
00062 ModelActuator::ModelActuator( World* world,
00063 Model* parent,
00064 const std::string& type ) :
00065 Model( world, parent, type ),
00066 goal(0),
00067 pos(0),
00068 max_speed(1),
00069 min_position(0),
00070 max_position(1),
00071 start_position(0),
00072 control_mode( CONTROL_VELOCITY ),
00073 actuator_type( TYPE_LINEAR ),
00074 axis(0,0,0)
00075 {
00076 this->SetWatts( WATTS_BASE );
00077
00078
00079
00080 this->SetBlobReturn(true);
00081
00082
00083
00084 }
00085
00086
00087 ModelActuator::~ModelActuator( void )
00088 {
00089
00090 }
00091
00092 void ModelActuator::Load( void )
00093 {
00094 Model::Load();
00095
00096
00097 if( wf->PropertyExists( wf_entity, "type" ) )
00098 {
00099 const std::string& type_str =
00100 wf->ReadString( wf_entity, "type", "linear" );
00101
00102 if( type_str == "linear" )
00103 actuator_type = TYPE_LINEAR;
00104 else if ( type_str == "rotational" )
00105 actuator_type = TYPE_ROTATIONAL;
00106 else
00107 {
00108 PRINT_ERR1( "invalid actuator type specified: \"%s\" - should be one of: \"linear\" or \"rotational\". Using \"linear\" as default.", type_str.c_str() );
00109 }
00110 }
00111
00112 if (actuator_type == TYPE_LINEAR)
00113 {
00114
00115 if( wf->PropertyExists( wf_entity, "axis" ) )
00116 {
00117 wf->ReadTuple( wf_entity, "axis", 0, 3, "fff", &axis.x, &axis.y, &axis.z );
00118
00119
00120 double length = sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z);
00121 if (length == 0)
00122 {
00123 PRINT_ERR( "zero length vector specified for actuator axis, using (1,0,0) instead" );
00124 axis.x = 1;
00125 }
00126 else
00127 {
00128 axis.x /= length;
00129 axis.y /= length;
00130 axis.z /= length;
00131 }
00132 }
00133 }
00134
00135 if( wf->PropertyExists( wf_entity, "max_speed" ) )
00136 {
00137 max_speed = wf->ReadFloat ( wf_entity, "max_speed", 1 );
00138 }
00139
00140 if( wf->PropertyExists( wf_entity, "max_position" ) )
00141 {
00142 max_position = wf->ReadFloat( wf_entity, "max_position", 1 );
00143 }
00144
00145 if( wf->PropertyExists( wf_entity, "min_position" ) )
00146 {
00147 min_position = wf->ReadFloat( wf_entity, "min_position", 0 );
00148 }
00149
00150 if( wf->PropertyExists( wf_entity, "start_position" ) )
00151 {
00152 start_position = wf->ReadFloat ( wf_entity, "start_position", 0 );
00153
00154 Pose DesiredPose = InitialPose;
00155
00156 cosa = cos(InitialPose.a);
00157 sina = sin(InitialPose.a);
00158
00159 switch (actuator_type)
00160 {
00161 case TYPE_LINEAR:
00162 {
00163 double cosa = cos(DesiredPose.a);
00164 double sina = sin(DesiredPose.a);
00165
00166 DesiredPose.x += (axis.x * cosa - axis.y * sina) * start_position;
00167 DesiredPose.y += (axis.x * sina + axis.y * cosa) * start_position;
00168 DesiredPose.z += axis.z * start_position;
00169 SetPose( DesiredPose );
00170 } break;
00171
00172 case TYPE_ROTATIONAL:
00173 {
00174 DesiredPose.a += start_position;
00175 SetPose( DesiredPose);
00176 }break;
00177 default:
00178 PRINT_ERR1( "unrecognized actuator type %d", actuator_type );
00179 }
00180 }
00181
00182 }
00183
00184 void ModelActuator::Update( void )
00185 {
00186 PRINT_DEBUG1( "[%lu] actuator update", 0 );
00187
00188
00189 double velocity = 0;
00190
00191
00192 Pose CurrentPose = GetPose();
00193
00194 Pose PoseDiff( CurrentPose.x - InitialPose.x,
00195 CurrentPose.y - InitialPose.y,
00196 CurrentPose.z - InitialPose.z,
00197 CurrentPose.a - InitialPose.a );
00198
00199 switch (actuator_type)
00200 {
00201 case TYPE_LINEAR:
00202 {
00203
00204
00205 pos = (PoseDiff.x * cosa - PoseDiff.y * sina)*axis.x + (PoseDiff.x * sina+ PoseDiff.y * cosa)* axis.y + PoseDiff.z * axis.z;
00206 } break;
00207 case TYPE_ROTATIONAL:
00208 {
00209 pos = PoseDiff.a;
00210 } break;
00211 default:
00212 PRINT_ERR1( "unrecognized actuator type %d", actuator_type );
00213 }
00214
00215
00216 if( this->subs )
00217 {
00218 switch( control_mode )
00219 {
00220 case CONTROL_VELOCITY :
00221 {
00222 PRINT_DEBUG( "actuator velocity control mode" );
00223 PRINT_DEBUG2( "model %s command(%.2f)",
00224 this->token,
00225 this->goal);
00226 if ((pos <= min_position && goal < 0) || (pos >= max_position && goal > 0))
00227 velocity = 0;
00228 else
00229 velocity = goal;
00230 } break;
00231
00232 case CONTROL_POSITION:
00233 {
00234 PRINT_DEBUG( "actuator position control mode" );
00235
00236
00237
00238 if (goal < min_position)
00239 goal = min_position;
00240 else if (goal > max_position)
00241 goal = max_position;
00242
00243 double error = goal - pos;
00244 velocity = error;
00245
00246 PRINT_DEBUG1( "error: %.2f\n", error);
00247
00248 }
00249 break;
00250
00251 default:
00252 PRINT_ERR1( "unrecognized actuator command mode %d", control_mode );
00253 }
00254
00255
00256
00257
00258
00259 if (velocity < -max_speed)
00260 velocity = -max_speed;
00261 else if (velocity > max_speed)
00262 velocity = max_speed;
00263
00264 Velocity outvel;
00265 switch (actuator_type)
00266 {
00267 case TYPE_LINEAR:
00268 {
00269 outvel.x = axis.x * velocity;
00270 outvel.y = axis.y * velocity;
00271 outvel.z = axis.z * velocity;
00272 outvel.a = 0;
00273 } break;
00274 case TYPE_ROTATIONAL:
00275 {
00276 outvel.x = outvel.y = outvel.z = 0;
00277 outvel.a = velocity;
00278 }break;
00279 default:
00280 PRINT_ERR1( "unrecognized actuator type %d", actuator_type );
00281 }
00282
00283
00284
00285
00286 PRINT_DEBUG4( " Set Velocity: [ %.4f %.4f %.4f %.4f ]\n",
00287 outvel.x, outvel.y, outvel.z, outvel.a );
00288 }
00289
00290 Model::Update();
00291 }
00292
00293 void ModelActuator::Startup( void )
00294 {
00295 Model::Startup();
00296
00297 PRINT_DEBUG( "position startup" );
00298 }
00299
00300 void ModelActuator::Shutdown( void )
00301 {
00302 PRINT_DEBUG( "position shutdown" );
00303
00304
00305 goal = 0;
00306
00307
00308
00309 Model::Shutdown();
00310 }
00311
00312 void ModelActuator::SetSpeed( double speed)
00313 {
00314 control_mode = CONTROL_VELOCITY;
00315 goal = speed;
00316 }
00317
00318
00319 void ModelActuator::GoTo( double pos)
00320 {
00321 control_mode = CONTROL_POSITION;
00322 goal = pos;
00323 }