MCA2 Puma Tutorial Chapter 6

From Mca2
Jump to: navigation, search



Add kinematics

Adding a kinematic module on top of the simulation

It is very uncomfortable to control every joint individually. Usually only position and orientation of the tool centre point (TCP) is of interest. If we want to control the TCP we need a kinematic module that calculates joint angle values from a specified desired TCP position and vice versa is able to calculate the current TCP position of a robot whose joint angle values are known.

As modules provide the two functions Control and Sense for up and down data flow we will implement the inverse and direct kinematics in these functions.

Defining IOs

After creating a new module named mKinematic we have to define the IOs. Our module will have 6 ControllerInputs for position and orientation of TCP and 6 ControllerOutputs for the 6 joint angles. On the other side our simulation provides the 6 simulated joint angles which are used to calculate the position and orientation of the TCP. Additionally we will get other informations from the direct kinematics. Beside others there is an output that indicates, whether the dedicated TCP input position is reachable or not. Then of course our inverse kinematics is not able to calculate corresponding joint angle values. Edit the file mKinematic.h:

     // this is a -*- C++ -*- file
/*!\file    Kinematic.h
 * \author   Kay-Ulrich Scholl
 * \date    06.11.02
 * $Revision: 1.1 $
 * $Id$
 * \brief   Contains tKinematic
 * \b tKinematic
 * Kinematic for PUMA robot. Just got this
 * from Hochholdinger and converted to MCA2.3
#ifndef _Kinematic_h_
#define _Kinematic_h_

// non MCA Includes - include with <>
// MCA Includes - include with ""
#include "kernel/tModule.h"
#include "kernel/Descr.h"

// Implementation

//! Short description of mKinematic
/*! Kinematics for PUMA robot
class mKinematic:public tModule
    Anonymous enumeration typ which contains the indices of the
    controller inputs.
  DESCR(static,mKinematic, ci_description,4,Natural,cDATA_VECTOR_END_MARKER);
  enum {
    eCI_TOOL_X,   /*! X-Coordinate of the position-vector */
    eCI_TOOL_Y,   /*! Y-Coordinate of the position-vector */
    eCI_TOOL_Z,   /*! Z-Coordinate of the position-vector */
    eCI_TOOL_O,   /*! Orientation angle of the tool [rad] */
    eCI_TOOL_A,   /*! Altitude angle of the tool [rad]    */
    eCI_TOOL_T,   /*! Tool angle of the tool [rad]        */
    eCI_ARM,      /*! Switch for the arm-orientation [left/right]    */
    eCI_ELBOW,    /*! Switch for the elbow-orientation [above/below] */
    eCI_WRIST,    /*! Switch for the wrist-orientation [down/up]     */
    eCI_FLIP,     /*! Toggle the other way round */
    eCI_DIMENSION /*!< Endmarker and Dimension */

    Anonymous enumeration typ which contains the indices of the
    controller outputs.
  DESCR(static,mKinematic, co_description,4,Natural,cDATA_VECTOR_END_MARKER);
  enum {
    eCO_WAIST,       /*! Angle of the waist-joint [rad]          */
    eCO_SHOULDER,    /*! Angle of the shoulder-joint [rad]       */
    eCO_ELBOW,       /*! Angle of the elbow-joint [rad]          */
    eCO_WRIST_ROT,   /*! Angle of the wrist_rotation-joint [rad] */
    eCO_WRIST_BEND,  /*! Angle of the wrist_bend-joint [rad]     */
    eCO_FLANGE,      /*! Angle of the flange-joint [rad]         */
    eCO_DIMENSION /*!< Endmarker and Dimension */

    Anonymous enumeration typ which contains the indices of the
    sensor inputs.
  DESCR(static,mKinematic, si_description,4,Natural,cDATA_VECTOR_END_MARKER);
  enum {
    eSI_WAIST,       /*! Angle of the waist-joint [rad]          */
    eSI_SHOULDER,    /*! Angle of the shoulder-joint [rad]       */
    eSI_ELBOW,       /*! Angle of the elbow-joint [rad]          */
    eSI_WRIST_ROT,   /*! Angle of the wrist_rotation-joint [rad] */
    eSI_WRIST_BEND,  /*! Angle of the wrist_bend-joint [rad]     */
    eSI_FLANGE,      /*! Angle of the flange-joint [rad]         */
    eSI_DIMENSION /*!< Endmarker and Dimension */

    Anonymous enumeration typ which contains the indices of the
    sensor outputs.
  DESCR(static,mKinematic, so_description,4,Natural,cDATA_VECTOR_END_MARKER);
  enum {
    eSO_TOOL_X,     /*! X-Coordinate of the position-vector */
    eSO_TOOL_Y,     /*! Y-Coordinate of the position-vector */
    eSO_TOOL_Z,     /*! Z-Coordinate of the position-vector */
    eSO_TOOL_O,     /*! Orientation angle of the tool [rad] */
    eSO_TOOL_A,     /*! Altitude angle of the tool [rad]    */
    eSO_TOOL_T,     /*! Tool angle of the tool [rad]        */
    eSO_ARM,        /*! Indicator of the arm-orientation [left/right]    */
    eSO_ELBOW,      /*! Indicator of the elbow-orientation [above/below] */
    eSO_WRIST,      /*! Indicator of the wrist-orientation [down/up]     */
    eSO_FLIP,       /*! Toggle the other way round */
    eSO_OVERFLOW,   /*! Indicator if the position is out of range */
    eSO_DIMENSION /*!< Endmarker and Dimension */

    Anonymous enumeration typ which contains the indices of the
  enum {
    ePAR_A2, /*!< (Denavit-Hartenberg Parameters) */
    ePAR_D2, /*!<            - " -                */
    ePAR_A3, /*!<            - " -                */
    ePAR_D4, /*!<            - " -                */
    ePAR_D6, /*!<            - " -                */
    /*! Min and Max range of the waist angles [rad]*/
    /*! Min and Max range of the shoulder angles [rad]*/
    /*! Min and Max range of the elbow angles [rad]*/
    /*! Min and Max range of the wrist-rotation angles [rad]*/
    /*! Min and Max range of the wrist-bend angles [rad]*/
    /*! Min and Max range of the flange angles [rad] */
    ePAR_DIMENSION /*!< Endmarker and Dimension */

   mKinematic initializes the Sensor-,ControllerIn and Output and the
    Parameters. The Parameters are also stored in our privat area.
  mKinematic(tParent *parent,
	     tDescription name="Kinematic",bool fixtit=true);
    Nothing to be done in the destructor
   In Controle is the inverse kinematics implemeted.
    If the angles are out of bounds or not calculatable, we set an
    overflow-flag and an reflector for the sense-function to give a Signal on
    SensorOut. The angles are kept until the new ones are in bounds.
  void Control();
    The Sense functions represents the direct kinematics.
    We calculate the direction of the arm, elbow and wrist.
    Also show the "overflow-case" if the position wanted in the inverse
    kinematic is not in range.
  void Sense();
  void Exception(tExceptionType type);
    This small helper-function sets the value to zero if it is near zero.
    This is necessary becaus of the atan2-function calculating with rounding
  void inline Small(long double *value);

    This function is true if the input-angle is in range
  bool InRange(long double *angle, long double *par_min, long double *par_max);

    convert the borders in the range from -PI to PI

  void Convert(long double *par_min, long double *par_max);

    The parameters are kept here for internal use

  // The Denavit-Hartenberg parameters:
  long double a2,d2,a3,d4,d6;

  // the range of the angles are stored here
  long double waist_min, waist_max;
  long double shoulder_min, shoulder_max;
  long double elbow_min, elbow_max;
  long double wrist_rot_min, wrist_rot_max;
  long double wrist_bend_min, wrist_bend_max;
  long double flange_min, flange_max;

  // keep the old angles here for fallback purpose
  long double d_temp[6];

  // We want to show the overflow
  long double overflow;

  // We have to reflect the overflow from Controle to Sense
  bool reflect;


Especially an inverse kinematics is not that easy to develop and we want to show the usage of MCA. Therefore the functionality of the kinematics is not explained here. Please have a look at the sources (at the end of this tutorial) if you are interested in how the the puma kinematics is working. Of course you need a mKinematics.cpp, you can download it from the next page.

At this point we just want to show you how the kinematic module can be integrated into our example programs. The basics (simulation) already exist and we just need another group that combines our simulation with the kinematics.

'''create a new group:''' -C projects/puma Kinematic

The rest will be set as wanted. This new group kinematic is the top group which already includes the puma_joints functions and of course the reply_value principles. If this was not the case we would have to create a new top group to combine the other groups.

All ControllerInput and SensorInput definitions of our group are identical to the ones of module mKinematic, as we want all upper IOs of the kinematics module available at MCAGUI. But we want also our 3D-model work further on. Therefore we need additionally the six joint angle values from the simulation. Look at the corresponding files for implementation details and especially notice the different possibilities how edges can be created.

All together

Finally we just need another part that starts our new kinematic group and another entry in our CMakeLists.txt:

INCLUDE (MCADescriptionBuilder)








// projects/mcap_puma/src/CMakeLists.txt

After compilation and start you can use MCAGUI and MCAbrowser to test the functionality and have a closer look at the internal representation of the modules.

You can load the old puma.mcagui if you go to the directory where it is saved and start mcagui there when the program is running by:

  mcagui puma.mcagui

You have to load the robot again and you have to set the new connections as well. While the robot is still connected to Waist Shoulder etc. the new LCDs and controllers are to be connected to the new configurations: x Pos y Pos z Pos and the three orientations. The LCD should immediately show the current positions of the robot. It is tricky to move the robot from these positions because only very small movements are allowed. You will have to figure out the scale. For example the Orientations do not run from 0 to 100 but from 0 to 2*Pi. These min max values can be set in the properties. (ctrl + middle mouse click).

Within the MCAbrowser all modules have a debug flag. Activate the debug flag of the kinematic module and look at the shell where PumaKinematic was started. The additional outputs result from the M_DEBUG lines in the Control and Sense functions. These output can be toggled manually. They are very helpful in finding module internal faults.


To get the full results you can carry out the following steps in your home directory:

  cd src/mca2/projects
  git clone git://

< Back=== Forward>

Personal tools