MCA2 Puma Tutorial Chapter 6

From Mca2
Revision as of 10:37, 1 June 2010 by Sutter (talk | contribs) (Defining IOs)
Jump to navigationJump to search

GO BACK HOME

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:

   /*!
     Anonymous enumeration type 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 type 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 type 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 type 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 */
   };

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.

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. 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 SConscript:

> # ------------------
> # puma_kinematic
> # Simulate all joints of a puma robot
> # and use a simple kinematic
> # ------------------
> pumakinematic = MCAProgram('puma_kinematic',jointsimulation.Build())
> pumakinematic.addSourceFiles("""
> pKinematic.cpp
> gKinematic.cpp
> gPumaJoints.cpp
> mJointSimulation3.cpp
> mKinematic.cpp
> """)
> pumakinematic.Build()

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.

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.