MCA2 Puma Tutorial Chapter 5

From Mca2
Jump to: navigation, search

GO BACK HOME

Contents

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/mcap_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, m_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, m_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 */
    };
 
 //gPumaJoints.h

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.cpp. 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. We will not use the bellow described Attribute tree but an xml file which was described in the calculator Tutorial. This time our xml file will not define the edges as those will be configured in group gPumaJoints.cpp. But it will give parameters to configure the program. For this we will write a xml file. To understand how an xml file works you could look at the following xml file example:

'''Example xml'''
<?xml version="1.0" encoding="UTF-8"?>
<Config>
 
<camera>
    <simulate>1</simulate>
    <cam_type>2</cam_type>
    <left_pic>data/5_markers_left.bmp</left_pic>
    <right_pic>data/5_markers_right.bmp</right_pic>
    <marker_detection>
      <near_clipping>1.0</near_clipping>
      <far_clipping>1000.0</far_clipping>
    </marker_detection>
  </camera>
</Config>

and then write it yourself with the information from the AttributeTree seen bellow. In the end it should look like this:

<?xml version="1.0" encoding="UTF-8"?>
<Config>
 
<mcap_puma>
    <joints>
    	<Waist>
    		<min>-160</min>
    		<max>160</max>
    		<v>64</v>
       	</Waist>
       	<Shoulder>
    		<min>-255</min>
    		<max>45</max>
    		<v>60</v>
       	</Shoulder>
       	<Elbow>
    		<min>-45</min>
    		<max>225</max>
    		<v>54</v>
       	</Elbow>
       	<Wrist_Rot>
    		<min>-110</min>
    		<max>170</max>
    		<v>60</v>
       	</Wrist_Rot>
       	<Wrist_Bend>
    		<min>-100</min>
    		<max>100</max>
    		<v>40</v>
       	</Wrist_Bend>
       	<Flange>
    		<min>-226</min>
    		<max>226</max>
    		<v>54</v>
       	</Flange>
	</joints>    
</mcap_puma>
</Config>

Warning xml files do not like spacing. Example: Write Wrist_Rot instead of Wrist Rot, otherwise the xml file will not be opened.

If you use the XML config file skip the section with the Attribute Tree and go on with section "Work with config files".

Configuration via Attribute Tree

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.

Work with config files

Resume working here if you chose the xml version

Add this to the constructor of the file gPumaJoints.cpp.

 //----------------------------------------------------------------------
 // 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();
}
 
 // gPumaJoints.cpp

Here you inserted a function Deg2Rad () which will not be defined if the following package is not included on top of gPumaJoints.cpp.:

  #include "mcal_math/OwnMath.h"

Change CMakeLists.txt

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

new_part.py -C projects/mcap_puma PumaJoints

and edit the generated file src/pPumaJoints.cpp so that a group gPumaJoints is instantiated.

The entry in our CMakeLists.txt file must then be:

INCLUDE (MCADescriptionBuilder)

ICMAKER_SET("mcap_puma" IDE_FOLDER ${MCAP_PUMA_IDE_FOLDER})

ICMAKER_ADD_SOURCES(
  pJointSimulation.cpp
  pPumaJoints.cpp
  mJointSimulation.cpp
  gPumaJoints.cpp
  )

ICMAKER_ADD_HEADERS(
  mJointSimulation.h
  gPumaJoints.h
  )

MCA_GENERATE_DESCRIPTIONS()

ICMAKER_LOCAL_CPPDEFINES(-DCALCULATOR_EXPORT_SYMBOLS)
ICMAKER_GLOBAL_CPPDEFINES(-D_IC_BUILDER_MCA2_CALCULATOR_)
ICMAKER_INCLUDE_DIRECTORIES(
  ${MCAL_KERNEL_INCLUDE_DIRS}
  ${MCAL_GENERAL_INCLUDE_DIRS}
  ${MCAP_PUMA_INCLUDE_DIRS}
  )

ICMAKER_INTERNAL_DEPENDENCIES(
  icl_core
  icl_core_config
  icl_core_logging
  mca2_kernel
  mca2_general
  mca2_math
  )

ICMAKER_BUILD_PROGRAM()

// projects/mcap_puma/src/CMakeLists.txt

After compilation, you can start the new program by running

puma_joints --configfile=projects/puma/etc/puma.AttributeTree

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.

< Back=== Forward>

Personal tools