MCA2 Puma Tutorial Chapter 3

From Mca2
Revision as of 10:18, 13 August 2009 by Karaman (talk | contribs) (Simulating a single joint)
Jump to navigationJump to search

GO BACK HOME

Simulating a single joint

Until now we have learned how to use the tool mcagui to create a user interface (front end). Now we shall have a look at how to implement back end controller software. So far the robot model does not really move in a realistic way. Real motors have a limited maximum velocity such that the joints cannot follow our slider movements instantaneously. Therefore we will now replace the guitest programmes with our first self-made back end. In this chapter we will develop a programme which simulates one joint of the robot. By starting this programme six times we yield the simulation of a complete 6-DOF robot.


Well prepared ?

The project directory is hopefully prepared as described at the beginning of this tutorial. We change into the main MCA2.4 directory and create our first module by:

> new_module.py -C projects/puma JointSimulation

The script newModule creates the two files mJointSimulation.h and mJointSimulation.C in the puma project directory (the leading "m" indicates that the files contain a MCA2 module). These files contain the definition and implementation of the class mJointSimulation, which is derived from tModule. Every module offers five IO interfaces: ControllerInput, ControllerOutput, SensorInput, SensorOutput and Parameters.

Warning

After creation new module files contain some default definitions and default code. This code has to be deleted in most cases, but is helpful as it shows how to use the interfaces and define parameters. Do not forget to delete the defaults when continuing the tutorial.

MCA2 parameters are used to change internal module operand during runtime. They are manipulated using the tool mcabrowser. We will learn how to use these later. At the moment we only need the interfaces ControllerInput and SensorOutput.

Well understandable descriptions

All MCA2 IO interfaces are represented by vectors. Particular values of these vectors can be accessed using indices. As our code should be maintained readable for others as well we will define all input and output indices in terms of enumerations. These enumerations are already prepared in the generated file mJointSimulation.h. You only need to add the necessary values for ControllerInputs (eCI) and SensorOutputs (eSO) of our new module:

>    /*!
>     Anonymous enumeration type which contains the indices of the
>     controller inputs.
>   */
>   _DESCR_( static, mJointSimulation::ci_description, 4, Natural, cDATA_VECTOR_END_MARKER );
>   enum {
>       eCI_DESIRED_JOINT_ANGLE,  /*!< desired angle value of joint */
>       eCI_DIMENSION             /*!< Endmarker and Dimension */
>   };
>   /*!
>     Anonymous enumeration type which contains the indices of the
>     sensor outputs.
>   */
>   _DESCR_( static, mJointSimulation::so_description, 4, Natural, cDATA_VECTOR_END_MARKER );
>   enum {
>       eSO_CURRENT_JOINT_ANGLE,  /*!< simulated joint angle value */
>       eSO_DIMENSION             /*!< Endmarker and Dimension */
>   };

Now our module can receive one controller input value and set one sensor output value.

As we don't need any sensor input or controller output we remove the entries in the according enumerations:

>   /*!
>     Anonymous enumeration type which contains the indices of the
>     controller outputs.
>    */
>   _DESCR_( static, mJointSimulation::co_description, 4, Natural, cDATA_VECTOR_END_MARKER );
>   enum {
>       eCO_DIMENSION /*!< Endmarker and Dimension */
>   };
>
>   /*!
>     Anonymous enumeration type which contains the indices of the
>     sensor inputs.
>    */
>   _DESCR_( static, mJointSimulation::si_description, 4, Natural, cDATA_VECTOR_END_MARKER );
>   enum {
>       eSI_DIMENSION /*!< Endmarker and Dimension */
>   };
	

Additionally we will define parameters (ePAR) which represent the joint's boundary values (minimal and maximal joint angle) and its angular velocity. For now only the names of the parameters have to be defined in mJointSimulation.h (do not forget to delete the default parameter definitions):

>   /*!
>   Anonymous enumeration type which contains the indices of the
>   parameters.
>   */
>   enum {
>       ePAR_MIN_ANGLE,          /*!< Minimum joint angle */
>       ePAR_MAX_ANGLE,          /*!< Maximum joint angle */
>       ePAR_ANGULAR_VELOCITY,   /*!< angular velocity of the joint */
>       ePAR_DIMENSION          /*!< Endmarker and Dimension */
>   };

If you wonder what eCI_DIMENSION, eSO_DIMENSION, and ePAR_DIMENSION are for: As they are always the last entry they specify the number of ControllerInputs, SensorOutputs, and Parameters respectively.

How to create and read in parameters will be explained in the next section.

All modules implement the methods Control and Sense which are invoked by the MCA2 main loop (See MCA Introduction). Method Control is meant to retrieve information from the ControllerInputs and compute the ControllerOutputs. Its counterpart Sense computes SensorOutput values from the module's SensorInputs. Both methods may modify the internal state of the module. At this point we want to implement a puma robot simulation which represents a virtual compound of sensors and actors. The simulation is passed ControllerInput values from which it computes simulated SensorOutput values which we will visualise in mcagui. This results in a shortened sensorimotor cycle in which we do not consider the SensorInputs and we need not compute ControllerOutputs. That way ControllerInput and SensorOutput are linked via the internal state of the module to be implemented.


Preparing parameters

Parameters can be manipulated using external tools like mcabrowser. In order to prevent accidental setting of inappropriate values parameters are well defined. The definition of numerical parameters (e.g. ParameterInt and ParameterFloat) requires the specification of a minimum and a maximum value. String parameters need to be limited to in length. In our example we only need floating point values. These parameters are created within the constructor of our module which is implemented in src/mJointSimulation.C. The order of the creation sequence has to match the enumeration definitions for the parameters in the header file (do not forget to delete the default code for creating parameters at this point).

>   InitParameter( ePAR_DIMENSION,
>                  new tParameterFloat( "min angle value", 0, 2 * M_PI, 0 ),        //min,max,value
>                  new tParameterFloat( "max angle value", 0, 2 * M_PI, 2 * M_PI ), //min,max,value
>                  new tParameterFloat( "angular velocity", 0, 100, 1 )             //min,max,value
>                );

Whenever a parameter value is changed the method Exception is invoked by the main programme with type eET_ParameterChanged as parameter. The module code frame already defines method Exception. The parameter transfer has to be implemented as listed below (here the default-code for reading parameters has to be deleted as well).

>       case eET_ParameterChanged:
>           INFOMSG( "%s>> Parameter changed.\n", Description() );
>           min_angle_value = ParameterFloat( ePAR_MIN_ANGLE );
>           max_angle_value = ParameterFloat( ePAR_MAX_ANGLE );
>           angular_velocity = ParameterFloat( ePAR_ANGULAR_VELOCITY );
>           ClearParameter();
>           break;

In our example we only use floating point parameters which are read out using method ParameterFloat. For different parameter types MCA provides methods ParameterBool, ParameterString, etc.

Now that we have defined the interface of our class we can start to write the joint simulation code.


Simulation of one joint

Our simulation is very simple: If we get a new desired angle value, we move the current angle value towards this desired value each time the control method is called considering the angular velocity. If the target value has been reached we stop. In realtime (RTAI/LXRT) environments the control and sense methods are called with a constant period of time. In other environments this period is not really constant. Therefore we have to determine the time elapsed between two calls manually. Considering the elapsed time and the angular velocity, the resulting simulated joint angle can be determined in a straightforward fashion from the current joint angle which we always hold in an internal state variable. As the desired angle value is passed to the module via the ControllerInput this functionality is implemented in the Control method. Delete the default code in the Control and the Sense method. Then put the following code into file mJointSimulation.C.

> //----------------------------------------------------------------------
> // class mJointSimulation Control
> //----------------------------------------------------------------------
> void mJointSimulation::Control() { if ( ControllerInputChanged() ) {
> // reset the changed flag of the controller input ClearControllerInput
> (); // get the desired angle: this->desired_joint_angle =
> ControllerInput( eCI_DESIRED_JOINT_ANGLE );
>       // Out of Min/Max
>       if ( this->desired_joint_angle > this->max_angle_value )
>       {
>           this->desired_joint_angle = this->max_angle_value;
>       }
>       else if ( this->desired_joint_angle < this->min_angle_value )
>       {
>           this->desired_joint_angle = this->min_angle_value;
>       }
>   }
>   // calculate the elapsed time from last access to control:
>   double elapsed_time = ( tTime::Now() - this->sense_timer ).ToMSec() * 0.001;
>   // reset timer:
>   this->sense_timer.FromNow();
>   if ( this->current_joint_angle != this->desired_joint_angle )
>   {


>       // not further than this:
>       double max_delta_angle = elapsed_time * angular_velocity;
>       // get the differences:
>       float diff = this->desired_joint_angle - this->current_joint_angle;
>       float sign = 1;
>       // decrease or increase the angle:
>       ( diff > 0 ) ? sign = 1 : sign = -1;
>       // too fast?
>       if ( fabs( diff ) > max_delta_angle )
>       {
>           diff = sign * max_delta_angle;
>       }
>       M_DEBUG( "current_joint_angle = %f, sign = %f, diff = %f\n", this->current_joint_angle, sign, diff );
>       float new_joint_angle = this->current_joint_angle + diff;
>       //save the new joint angle in a state variable
>       this->current_joint_angle = new_joint_angle;
>       this->current_joint_angle_changed = true;
>   }
>   else
>   {
>       this->current_joint_angle_changed = false;
>   }
> }
> //----------------------------------------------------------------------
> // class mJointSimulation Sense
> //----------------------------------------------------------------------
> void mJointSimulation::Sense ()
> {
>   if ( this->current_joint_angle_changed )
>   {
>       SensorOutput( eSO_CURRENT_JOINT_ANGLE ) = this->current_joint_angle;
>       SetSensorOutputChanged ();
>   }
>   else
>   {
>       ClearSensorOutput ();
>   }
> }

Additionally add the following line to the constructor of our module src/mJointSimulation.C in order to set the timer for the first time and for initialisation of class variables:

>   this->sense_timer.FromNow();
>   this->desired_joint_angle = 0.;
>   this->current_joint_angle = 0.;
>   this->current_joint_angle_changed = false;

In order to avoid unnecessary code invocation, flags have been introduced to indicate changes of ControllerInput and SensorInput.

Whenever a module changes its SensorOutput or ControllerOutput values this fact has to be indicated to other modules by calling method SetControllerOutputChanged or SetSensorOutputChanged respectively. In case nothing has been changed method ClearControllerOutput (ClearSensorOutput respectively) has to be called to clear the corresponding flag.

The flags indicating ControllerInput and SensorInput changes can be read out using the methods ControllerInputChanged and SensorInputChanged and are reset by ClearControllerInput and ClearSensorInput. In most cases modules only need to do something if the input values have changed. Note that our example represents a special case as we have implemented a simulation module.

To complete our first module we still have to introduce some internal state variables which have to be declared in the header file mJointSimulation.h:

> private:
>    float desired_joint_angle;
>    float current_joint_angle;
>    bool current_joint_angle_changed;
>    float min_angle_value;
>    float max_angle_value;
>    float angular_velocity;
>    tTime sense_timer;

Class tTime is declared in the header file kernel/tTime.h which is included by kernel/tModule.h. As our module inherits from tModule no further includes are necessary in our header file.