model_actuator.cc

///////////////////////////////////////////////////////////////////////////
//
// File: model_laser.c
// Author: Richard Vaughan
// Date: 10 June 2004
//
// CVS info:
//  $Source: /home/tcollett/stagecvs/playerstage-cvs/code/stage/libstage/model_position.cc,v $
//  $Author: rtv $
//  $Revision$
//
///////////////////////////////////////////////////////////////////////////


#include <sys/time.h>
#include <math.h>
#include <stdlib.h>

//#define DEBUG
#include "stage.hh"
#include "worldfile.hh"

using namespace Stg;

/**
@ingroup model
@defgroup model_actuator Actuator model

The actuator model simulates a simple actuator, where child objects can be displaced in a
single dimension, or rotated about the Z axis.

API: Stg::ModelActuator

<h2>Worldfile properties</h2>

@par Summary and default values

@verbatim
actuator
(
  # actuator properties
  type ""
  axis [x y z]

  min_position
  max_position
  max_speed
)
@endverbatim

@par Details

- type "linear" or "rotational"\n
  the style of actuator. Rotational actuators may only rotate about the z axis (i.e. 'yaw')
- axis
  if a linear actuator the axis that the actuator will move along
 */
  
static const double WATTS_KGMS = 5.0; // cost per kg per meter per second
static const double WATTS_BASE = 2.0; // base cost of position device

ModelActuator::ModelActuator( World* world,
										Model* parent,
										const std::string& type ) : 
  Model( world, parent, type ),
  goal(0), 
  pos(0), 
  max_speed(1), 
  min_position(0), 
  max_position(1),
  start_position(0),
  control_mode( CONTROL_VELOCITY ),
  actuator_type( TYPE_LINEAR ),
  axis(0,0,0)
{
  this->SetWatts( WATTS_BASE );
  
  // sensible position defaults
  this->SetVelocity( Velocity(0,0,0,0) );
  this->SetBlobReturn(true);  

  // Allow the models to move
  VelocityEnable();
}


ModelActuator::~ModelActuator( void )
{
	// nothing to do
}

void ModelActuator::Load( void )
{
  Model::Load();

  // load steering mode
  if( wf->PropertyExists( wf_entity, "type" ) )
	 {
		const std::string&  type_str =
		  wf->ReadString( wf_entity, "type", "linear" );
		
		if( type_str == "linear" )
		  actuator_type = TYPE_LINEAR;
		else if ( type_str ==  "rotational" )
		  actuator_type = TYPE_ROTATIONAL;
		else
			{
			  PRINT_ERR1( "invalid actuator type specified: \"%s\" - should be one of: \"linear\" or \"rotational\". Using \"linear\" as default.", type_str.c_str() );
			}
	 }
    
	if (actuator_type == TYPE_LINEAR)
	{
		// if we are a linear actuator find the axis we operate in
		if( wf->PropertyExists( wf_entity, "axis" ) )
		{
			axis.x = wf->ReadTupleLength( wf_entity, "axis", 0, 0 );
			axis.y = wf->ReadTupleLength( wf_entity, "axis", 1, 0 );
			axis.z = wf->ReadTupleLength( wf_entity, "axis", 2, 0 );

			// normalise the axis
			double length = sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z);
			if (length == 0)
			{
				PRINT_ERR( "zero length vector specified for actuator axis, using (1,0,0) instead" );
				axis.x = 1;
			}
			else
			{
				axis.x /= length;
				axis.y /= length;
				axis.z /= length;
			}
		}
	}

	if( wf->PropertyExists( wf_entity, "max_speed" ) )
	{
		max_speed = wf->ReadFloat ( wf_entity, "max_speed", 1 );
	}

	if( wf->PropertyExists( wf_entity, "max_position" ) )
	{
		max_position = wf->ReadFloat( wf_entity, "max_position", 1 );
	}

	if( wf->PropertyExists( wf_entity, "min_position" ) )
	{
		min_position = wf->ReadFloat( wf_entity, "min_position", 0 );
	}

	if( wf->PropertyExists( wf_entity, "start_position" ) )
	{
		start_position = wf->ReadFloat ( wf_entity, "start_position", 0 );
		
		Pose DesiredPose = InitialPose;
		
		cosa = cos(InitialPose.a);
		sina = sin(InitialPose.a);
		
		switch (actuator_type)
		  {
		  case TYPE_LINEAR:
			 {				 
				double cosa = cos(DesiredPose.a);
				double sina = sin(DesiredPose.a);
				
				DesiredPose.x += (axis.x * cosa - axis.y * sina) * start_position;
				DesiredPose.y += (axis.x * sina + axis.y * cosa) * start_position;
				DesiredPose.z += axis.z * start_position;
				SetPose( DesiredPose );
			 } break;

		  case TYPE_ROTATIONAL:
			 {
				DesiredPose.a += start_position;
				SetPose( DesiredPose);
			 }break;
			default:
			  PRINT_ERR1( "unrecognized actuator type %d", actuator_type );
		  }
	}

}

void ModelActuator::Update( void  )
{
	PRINT_DEBUG1( "[%lu] actuator update", 0 );

	// stop by default
	double velocity = 0;

	// update current position
	Pose CurrentPose = GetPose();
	// just need to get magnitude difference;
	Pose PoseDiff( CurrentPose.x - InitialPose.x,
						CurrentPose.y - InitialPose.y,
						CurrentPose.z - InitialPose.z,
						CurrentPose.a - InitialPose.a );

	switch (actuator_type)
	{
		case TYPE_LINEAR:
		{
			// When the velocity is applied, it will automatically be rotated by the angle the model is at
			// So, rotate the axis of movement by the model angle before doing a dot product to find the actuator position
			pos = (PoseDiff.x * cosa - PoseDiff.y * sina)*axis.x + (PoseDiff.x * sina+ PoseDiff.y * cosa)* axis.y + PoseDiff.z * axis.z;
		} break;
		case TYPE_ROTATIONAL:
		{
			pos = PoseDiff.a;
		} break;
		default:
			PRINT_ERR1( "unrecognized actuator type %d", actuator_type );
	}


	if( this->subs )   // no driving if noone is subscribed
	{
		switch( control_mode )
		{
			case CONTROL_VELOCITY :
				{
					PRINT_DEBUG( "actuator velocity control mode" );
					PRINT_DEBUG2( "model %s command(%.2f)",
							this->token,
							this->goal);
					if ((pos <= min_position && goal < 0) || (pos >= max_position && goal > 0))
						velocity = 0;
					else
						velocity = goal;
				} break;

			case CONTROL_POSITION:
				{
					PRINT_DEBUG( "actuator position control mode" );


					// limit our axis
					if (goal < min_position)
						goal = min_position;
					else if (goal > max_position)
						goal = max_position;

					double error = goal - pos;
					velocity = error;

					PRINT_DEBUG1( "error: %.2f\n", error);

				}
				break;

			default:
				PRINT_ERR1( "unrecognized actuator command mode %d", control_mode );
		}

		// simple model of power consumption
		//TODO power consumption

		// speed limits for controller
		if (velocity < -max_speed)
			velocity = -max_speed;
		else if (velocity > max_speed)
			velocity = max_speed;

		Velocity outvel;
		switch (actuator_type)
		{
			case TYPE_LINEAR:
			{
				outvel.x = axis.x * velocity;
				outvel.y = axis.y * velocity;
				outvel.z = axis.z * velocity;
				outvel.a = 0;
			} break;
			case TYPE_ROTATIONAL:
			{
				outvel.x = outvel.y = outvel.z = 0;
				outvel.a = velocity;
			}break;
			default:
				PRINT_ERR1( "unrecognized actuator type %d", actuator_type );
		}

		this->SetVelocity( outvel );
		//this->GPoseDirtyTree();
		PRINT_DEBUG4( " Set Velocity: [ %.4f %.4f %.4f %.4f ]\n",
				outvel.x, outvel.y, outvel.z, outvel.a );
	}

	Model::Update();
}

void ModelActuator::Startup( void )
{
	Model::Startup();

	PRINT_DEBUG( "position startup" );
}

void ModelActuator::Shutdown( void )
{
	PRINT_DEBUG( "position shutdown" );

	// safety features!
	goal = 0;

	velocity.Zero();

	Model::Shutdown();
}

void ModelActuator::SetSpeed( double speed)
{
	control_mode = CONTROL_VELOCITY;
	goal = speed;
}


void ModelActuator::GoTo( double pos)
{
	control_mode = CONTROL_POSITION;
	goal = pos;
}