MCA2 Puma Tutorial Chapter 5

From Mca2
Revision as of 11:48, 14 August 2009 by Karaman (talk | contribs)
Jump to navigationJump to search

GO BACK HOME

Simulating a complete Puma robot

A main feature of MCA is the possibility to combine individual modules to groups. These groups then act like any other individual module and can also be integrated into other more complex groups. In this chapter we will learn how to integrate our six joint simulation modules into one group, which then simulates a complete puma robot.

Create a group

The script newModule creates empty but useful module definitions. There is an equivalent script for groups, too: new_group.py Just type:

> new_group.py -C projects/puma PumaJoints

in your MCA main directory.

In the new header file gPumaJoints.h ("g" for "group") the necessary modules have to be included:

> //----------------------------------------------------------------------
> // Project Includes - include with ""
> //----------------------------------------------------------------------
> #include "mJointSimulation.h"

Additionally the IO definitions have to be added. We use a controller input and a sensor output for each included joint module:

>    /*!
>    Anonymous enumeration type which contains the indices of the
>    controller inputs.
>    */
>    _DESCR_( static, gPumaJoints::ci_description, 4, Natural, cDATA_VECTOR_END_MARKER );
>    enum {
>        eCI_WAIST,
>        eCI_SHOULDER,
>        eCI_ELBOW,
>        eCI_WRIST_ROT,
>        eCI_WRIST_BEND,
>        eCI_FLANGE,
>        eCI_DIMENSION /*!< Endmarker and Dimension */
>    };
>    /*!
>      Anonymous enumeration type which contains the indices of the
>      sensor outputs.
>     */
>    _DESCR_( static, gPumaJoints::so_description, 4, Natural, cDATA_VECTOR_END_MARKER );
>    enum {
>        eSO_WAIST,
>        eSO_SHOULDER,
>        eSO_ELBOW,
>        eSO_WRIST_ROT,
>        eSO_WRIST_BEND,
>        eSO_FLANGE,
>        eSO_DIMENSION /*!< Endmarker and Dimension */
>    };

The functions Control and Sense of groups are already implemented. They call the equivalent functions of the integrated modules in an iterative way. You as user only have to create modules or groups within the constructor and if necessary release used heap memory in the destructor. All modules that are integrated into a group are deleted automatically within the destructor of tModule .

In our case we have to create six instances of the module mJointSimulation in the constructor of gPumaJoints.C. Then the IOs of these modules have to be connected to the IOs of our group. AddEdge... commands are used to do this.

> gPumaJoints::gPumaJoints( tParent *parent, tDescription name, bool fixit )
>        : tGroup( parent, name,
>                  eSI_DIMENSION, eSO_DIMENSION, eCI_DIMENSION, eCO_DIMENSION,
>                  si_description, so_description, ci_description, co_description )
>{
>    // create module
>    mJointSimulation *waist = new mJointSimulation( this, "Waist" );
>    // Add Edges
>    AddEdgeDown( this, waist, 1, eCI_WAIST, mJointSimulation::eCI_DESIRED_JOINT_ANGLE );
>    AddEdgeUp( waist, this, 1, mJointSimulation::eSO_CURRENT_JOINT_ANGLE, eSO_WAIST );
>
>    mJointSimulation *shoulder = new mJointSimulation( this, "Shoulder" );
>    AddEdgeDown( this, shoulder, 1, eCI_SHOULDER, mJointSimulation::eCI_DESIRED_JOINT_ANGLE );
>    AddEdgeUp( shoulder, this, 1, mJointSimulation::eSO_CURRENT_JOINT_ANGLE, eSO_SHOULDER );
>
>    mJointSimulation *elbow = new mJointSimulation( this, "Elbow" );
>    AddEdgeDown( this, elbow, 1, eCI_ELBOW, mJointSimulation::eCI_DESIRED_JOINT_ANGLE );
>    AddEdgeUp( elbow, this, 1, mJointSimulation::eSO_CURRENT_JOINT_ANGLE, eSO_ELBOW );
>
>    mJointSimulation *wrist_rot = new mJointSimulation( this, "Wrist Rot" );
>    AddEdgeDown( this, wrist_rot, 1, eCI_WRIST_ROT, mJointSimulation::eCI_DESIRED_JOINT_ANGLE );
>    AddEdgeUp( wrist_rot, this, 1, mJointSimulation::eSO_CURRENT_JOINT_ANGLE, eSO_WRIST_ROT );
>
>    mJointSimulation *wrist_bend = new mJointSimulation( this, "Wrist Bend" );
>    AddEdgeDown( this, wrist_bend, 1, eCI_WRIST_BEND, mJointSimulation::eCI_DESIRED_JOINT_ANGLE );
>    AddEdgeUp( wrist_bend, this, 1, mJointSimulation::eSO_CURRENT_JOINT_ANGLE, eSO_WRIST_BEND );
>
>    mJointSimulation *flange = new mJointSimulation( this, "Flange" );
>    AddEdgeDown( this, flange, 1, eCI_FLANGE, mJointSimulation::eCI_DESIRED_JOINT_ANGLE );
>    AddEdgeUp( flange, this, 1, mJointSimulation::eSO_CURRENT_JOINT_ANGLE, eSO_FLANGE );
>
>    if ( fixit )
>        FixIt();
>}

In the above example we use one form of the AddEdge... functions. We define the from and to modules and the number of values that are to be copied. As our modules only have one value on each side, we connect only one. The last two parameters specify the index of the inputs and outputs to be connected.

Tip: Always take care to set the correct number of edges to be connected in the AddEdge... function. Otherwise either data is not transferred or a segfault might occur.

Configuration files

We would need 24 parameters to configure our new program. This is confusing, unclear and very uncomfortable. At this point we learn about the possibility to use configuration files in MCA. Configuration files use tAttributeTree to read, store (and save) parameter attributes. An attribute tree is organised like the directory structure of a file system: Every entry has an attribute name and an attribute content. The content may be empty. Moreover every entry may have subentries. When saved to file, subentries are included into { } brackets, or their names include the parent entry name separated by a point:

> {
>   example_entry{
>     sub_entry: entry content
>  }example_entry
>  example_entry_2: entry 2 content
>  example_entry_2.sub_entry: hello world 
> }

Such configuration files can be easily be created manually with every editor you like.

In our example, we need an AttributeTree file that contains all necessary parameters for our 6 joint simulation modules. Of course there are many possibilities to define them. Here's one (save this as etc/puma.AttributeTree):

> puma{
>     joints{
>         Waist{
>             min:-160
>             max:160
>             v:64
>         }Waist
>         Shoulder{
>             min:-255
>             max:45
>             v:60
>         }Shoulder
>         Elbow{
>             min:-45
>             max:225
>             v:54
>         }Elbow
>         Wrist Rot{
>             min:-110
>             max:170
>             v:60
>         }Wrist Rot
>         Wrist Bend{
>             min:-100
>             max:100
>             v:40
>         }Wrist Bend
>         Flange{
>             min:-266
>             max:266
>             v:54
>         }Flange
>     }joints
> }puma

If a program is started with parameter --configfile=etc/puma.AttributeTree, the part reads the file and stores all attributes in an internal structure. As all modules can access the part via their parents (top level parent is always a part) they are all able to ask for attributes. The following example shows how to access and use attributes from a global configuration file. Add this to the constructor of the file gPumaJoints.C.

> //----------------------------------------------------------------------
> // class gPumaJoints
> //----------------------------------------------------------------------
> gPumaJoints::gPumaJoints( tParent *parent, tDescription name, bool fixit )
>         : tGroup( parent, name,
>                   eSI_DIMENSION, eSO_DIMENSION, eCI_DIMENSION, eCO_DIMENSION,
>                   si_description, so_description, ci_description, co_description )
> {
>     float waist_min, waist_max, waist_v;
>     float shoulder_min, shoulder_max, shoulder_v;
>     float elbow_min, elbow_max, elbow_v;
>     float wrist_rot_min, wrist_rot_max, wrist_rot_v;
>     float wrist_bend_min, wrist_bend_max, wrist_bend_v;
>     float flange_min, flange_max, flange_v;
> 
>     tAttributesStruct attributes[] = { tAttributeFloat( "puma.joints.Waist.min", waist_min ),
>                                        tAttributeFloat( "puma.joints.Waist.max", waist_max ),
>                                        tAttributeFloat( "puma.joints.Waist.v", waist_v ),
>                                        tAttributeFloat( "puma.joints.Shoulder.min", shoulder_min ),
>                                        tAttributeFloat( "puma.joints.Shoulder.max", shoulder_max ),
>                                        tAttributeFloat( "puma.joints.Shoulder.v", shoulder_v ),
>                                        tAttributeFloat( "puma.joints.Elbow.min", elbow_min ),
>                                        tAttributeFloat( "puma.joints.Elbow.max", elbow_max ),
>                                        tAttributeFloat( "puma.joints.Elbow.v", elbow_v ),
>                                        tAttributeFloat( "puma.joints.Wrist Rot.min", wrist_rot_min ),
>                                        tAttributeFloat( "puma.joints.Wrist Rot.max", wrist_rot_max ),
>                                        tAttributeFloat( "puma.joints.Wrist Rot.v", wrist_rot_v ),
>                                        tAttributeFloat( "puma.joints.Wrist Bend.min", wrist_bend_min ),
>                                        tAttributeFloat( "puma.joints.Wrist Bend.max", wrist_bend_max ),
>                                        tAttributeFloat( "puma.joints.Wrist Bend.v", wrist_bend_v ),
>                                        tAttributeFloat( "puma.joints.Flange.min", flange_min ),
>                                        tAttributeFloat( "puma.joints.Flange.max", flange_max ),
>                                        tAttributeFloat( "puma.joints.Flange.v", flange_v ),
>                                        tAttributeFieldEnd()
>                                      };
> 
>     if ( !GetAttributes( attributes, 0, true ) )
>     {
>         ERRORMSG( "%s could not be initialized because of missing attributes!\n", Description() );
>         SetStatus( eERROR );
>         return ;
>     }
> 
>     // create module
>     mJointSimulation *waist = new mJointSimulation( this, "Waist" );
>     // Add Edges
>     AddEdgeDown( this, waist, 1, eCI_WAIST, mJointSimulation::eCI_DESIRED_JOINT_ANGLE );
>     AddEdgeUp( waist, this, 1, mJointSimulation::eSO_CURRENT_JOINT_ANGLE, eSO_WAIST );
>     waist->SetParameters( mJointSimulation::ePAR_DIMENSION,
>                           mJointSimulation::ePAR_MIN_ANGLE, Deg2Rad( waist_min ),
>                           mJointSimulation::ePAR_MAX_ANGLE, Deg2Rad( waist_max ),
>                           mJointSimulation::ePAR_ANGULAR_VELOCITY, Deg2Rad( waist_v ) );
> 
>     mJointSimulation *shoulder = new mJointSimulation( this, "Shoulder" );
>     AddEdgeDown( this, shoulder, 1, eCI_SHOULDER, mJointSimulation::eCI_DESIRED_JOINT_ANGLE );
>     AddEdgeUp( shoulder, this, 1, mJointSimulation::eSO_CURRENT_JOINT_ANGLE, eSO_SHOULDER );
>     shoulder->SetParameters( mJointSimulation::ePAR_DIMENSION,
>                              mJointSimulation::ePAR_MIN_ANGLE, Deg2Rad( shoulder_min ),
>                              mJointSimulation::ePAR_MAX_ANGLE, Deg2Rad( shoulder_max ),
>                              mJointSimulation::ePAR_ANGULAR_VELOCITY, Deg2Rad( shoulder_v ) );
> 
>     mJointSimulation *elbow = new mJointSimulation( this, "Elbow" );
>     AddEdgeDown( this, elbow, 1, eCI_ELBOW, mJointSimulation::eCI_DESIRED_JOINT_ANGLE );
>     AddEdgeUp( elbow, this, 1, mJointSimulation::eSO_CURRENT_JOINT_ANGLE, eSO_ELBOW );
>     elbow->SetParameters( mJointSimulation::ePAR_DIMENSION,
>                           mJointSimulation::ePAR_MIN_ANGLE, Deg2Rad( elbow_min ),
>                           mJointSimulation::ePAR_MAX_ANGLE, Deg2Rad( elbow_max ),
>                           mJointSimulation::ePAR_ANGULAR_VELOCITY, Deg2Rad( elbow_v ) );
> 
>     mJointSimulation *wrist_rot = new mJointSimulation( this, "Wrist Rot" );
>     AddEdgeDown( this, wrist_rot, 1, eCI_WRIST_ROT, mJointSimulation::eCI_DESIRED_JOINT_ANGLE );
>     AddEdgeUp( wrist_rot, this, 1, mJointSimulation::eSO_CURRENT_JOINT_ANGLE, eSO_WRIST_ROT );
>     wrist_rot->SetParameters( mJointSimulation::ePAR_DIMENSION,
>                               mJointSimulation::ePAR_MIN_ANGLE, Deg2Rad( wrist_rot_min ),
>                               mJointSimulation::ePAR_MAX_ANGLE, Deg2Rad( wrist_rot_max ),
>                               mJointSimulation::ePAR_ANGULAR_VELOCITY, Deg2Rad( wrist_rot_v ) );
> 
>     mJointSimulation *wrist_bend = new mJointSimulation( this, "Wrist Bend" );
>     AddEdgeDown( this, wrist_bend, 1, eCI_WRIST_BEND, mJointSimulation::eCI_DESIRED_JOINT_ANGLE );
>     AddEdgeUp( wrist_bend, this, 1, mJointSimulation::eSO_CURRENT_JOINT_ANGLE, eSO_WRIST_BEND );
>     wrist_bend->SetParameters( mJointSimulation::ePAR_DIMENSION,
>                                mJointSimulation::ePAR_MIN_ANGLE, Deg2Rad( wrist_bend_min ),
>                                mJointSimulation::ePAR_MAX_ANGLE, Deg2Rad( wrist_bend_max ),
>                                mJointSimulation::ePAR_ANGULAR_VELOCITY, Deg2Rad( wrist_bend_v ) );
> 
> 
>     mJointSimulation *flange = new mJointSimulation( this, "Flange" );
>     AddEdgeDown( this, flange, 1, eCI_FLANGE, mJointSimulation::eCI_DESIRED_JOINT_ANGLE );
>     AddEdgeUp( flange, this, 1, mJointSimulation::eSO_CURRENT_JOINT_ANGLE, eSO_FLANGE );
>     flange->SetParameters( mJointSimulation::ePAR_DIMENSION,
>                            mJointSimulation::ePAR_MIN_ANGLE, Deg2Rad( flange_min ),
>                            mJointSimulation::ePAR_MAX_ANGLE, Deg2Rad( flange_max ),
>                            mJointSimulation::ePAR_ANGULAR_VELOCITY, Deg2Rad( flange_v ) );
> 
>     if ( fixit )
>         FixIt();
> }


Change SConscript

There are several other possibilities in order to get attributes from the part.

Of course we need another main program, where our group gPumaJoints is used as top-level module instead of mJointSimulation. We also no longer need the prompt parameters, as we read the values from the configuration file.

So make a new part using the script newPart project/puma ThirdPart and edit the generated file src/pPumaJoints.C so that a group gPumaJoints is instantiated. The entry in our SConscript file must then be:

> # ------------------
> # puma_joints with parameters
> # Simulate all joints of a puma robot
> # ------------------
> pumajoints = MCAProgram('puma_joints',jointsimulation.Build())
> pumajoints.addSourceFiles("""
> pPumaJoints.C
> gPumaJoints.C
> mJointSimulation.C
> """)
> 
> pumajoints.Build()

After compilation, you can start the new program and have a closer look with MCAbrowser to it. When controlling the robot with MCAGUI, you have to reconnect the IOs, as they now have individual descriptions on program side.