MCA2 Puma Tutorial Chapter 5
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.
