robotgl.cpp

Go to the documentation of this file.

/*
Copyright (C) 2002-2003  Etienne Lachance

This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA


Report problems and direct all questions to:

email: etienne.lachance@polymtl.ca or richard.gourdeau@polymtl.ca
*/



static const char rcsid[] = "$Id: robotgl.cpp,v 1.10 2006/05/16 16:46:18 gourdeau Exp $";

#include "robotgl.h"

//--------------------------- B A S I C  R O B O T  O P E N G L ----------------------

Robotgl_basic::Robotgl_basic(const short dof_): Basic_object(), dof(dof_)
{
    try
    {
        linkgl = new Linkgl[dof];
        linkgl--;
        jointgl = new Jointgl[dof];
        jointgl--;
        axis = new Axis(0.15);
    }
    catch(bad_alloc)
    {
        cerr << "Robotgl_basic constructor : new ran out of memory" << endl;
        exit(1);
    }
}

Robotgl_basic::Robotgl_basic(const Robotgl_basic & x): Basic_object(x)
{
    axis = x.axis;
    pos = x.pos;

    if ( dof != x.dof )
    {
        dof = x.dof;
        linkgl++;
        jointgl++;
        delete[] jointgl;
        delete[] linkgl;

        try
        {
            linkgl = new Linkgl[dof];
            linkgl--;
            jointgl = new Jointgl[dof];
            jointgl--;
        }
        catch(bad_alloc)
        {
            cerr << "Robotgl_basic constructor : new ran out of memory" << endl;
            exit(1);
        }
    }

    for(int i = 1; i <= dof; i++)
    {
        linkgl[i] = x.linkgl[i];
        jointgl[i] = x.jointgl[i];
    }
}

Robotgl_basic::~Robotgl_basic()
{
    linkgl++;
    jointgl++;
    delete[] jointgl;
    delete[] linkgl;
    delete axis;
}

Robotgl_basic & Robotgl_basic::operator=(const Robotgl_basic & x)
{
    Basic_object::operator=(x);

    pos = x.pos;
    axis = x.axis;

    linkgl++;
    jointgl++;
    delete[]linkgl;
    delete[] jointgl;

    if(dof != x.dof)
    {
        dof = x.dof;
/*      
        linkgl++;
        delete[] linkgl;
        jointgl++;
        delete[] jointgl;
*/
        try
        {
            linkgl = new Linkgl[dof];
            linkgl--;
            jointgl = new Jointgl[dof];
            jointgl--;
        }
        catch(bad_alloc)
        {
            cerr << "Robotgl_basic constructor : new ran out of memory" << endl;
            exit(1);
        }
    }

    for(int i = 1; i <= dof; i++)
    {
        linkgl[i] = x.linkgl[i];
        jointgl[i] = x.jointgl[i];
    }

   return (*this);
}

void Robotgl_basic::set_dof(const short dof_)
{
    if ( dof != dof_ )
    {
        dof = dof_;
        linkgl++;
        jointgl++;
        delete[] jointgl;
        delete[] linkgl;
        try
        {
            linkgl = new Linkgl[dof];
            linkgl--;
            jointgl = new Jointgl[dof];
            jointgl--;
        }
        catch(bad_alloc)
        {
            cerr << "Robotgl_basic constructor : new ran out of memory" << endl;
            exit(1);
        }
    }
}

bool Robotgl_basic::Is_STL()
{
    for(int i = 1; i <= dof; i++)
    {
        if(linkgl[i].STL || linkgl[i].STL)
            return true;
    }
    return false;
}

void Robotgl_basic::create_list()
{
    for(int i = 1; i <= dof; i++)
    {
        jointgl[i].create_list();
        linkgl[i].create_list();
    }
}


//--------------------------- R O B O T  DH  O P E N G L -----------------------------

Robotgl::Robotgl(short ndof) : Robot(ndof), Robotgl_basic(ndof)
{
    message_object = "Robotgl";
}

Robotgl::Robotgl(const Matrix & initrob, const ColumnVector & position_) : 
    Robot(initrob),  Robotgl_basic()
{
    std::vector<double> parameters;
    for(int i = 1; i <= initrob.Nrows(); i++)
    {
        parameters.push_back(initrob(i,1));   // joint type
        parameters.push_back(initrob(i,2));   // DH -> theta
        parameters.push_back(initrob(i,5));   // DH -> alpha
        parameters.push_back(initrob(i,4));   // DH -> a
        parameters.push_back(initrob(i,3));   // DH -> d
        parameters.push_back(0.05);           // link radius
    }

    position = position_;
    pos = ColumnVector(3); pos = 0;
    
    message_object = "Robotgl";
    Robotgl_basic::set_dof(Robot::dof);

    std::vector<double> para;
    for(int j = 1; j <= Robot_basic::dof; j++)
    {
        para.clear();              // Clear all parameters for next link
        for(int i = (j-1)*NbParametresPerLink; i < j*NbParametresPerLink; i++)
          para.push_back(parameters[i]);

        jointgl[j] = Jointgl(para, position, true);
        linkgl[j] = Linkgl(para, position, jointgl[j].radius, true);
    }
}

Robotgl::Robotgl(const Matrix & initrob, const Matrix & initrobm,
                 const ColumnVector & position_) : Robot(initrob, initrobm),
                                                   Robotgl_basic()
{
    std::vector<double> parameters;;
    for(int i = 1; i <= initrob.Nrows(); i++)
    {
        parameters.push_back(initrob(i,1));   // joint type
        parameters.push_back(initrob(i,2));   // DH -> theta
        parameters.push_back(initrob(i,5));   // DH -> alpha
        parameters.push_back(initrob(i,4));   // DH -> a
        parameters.push_back(initrob(i,3));   // DH -> d
        parameters.push_back(0.05);           // link radius
    }

    position = position_;
    pos = ColumnVector(3); pos = 0;

    Robotgl_basic::set_dof(Robot::dof);

    std::vector<double> para;
    for(int j = 1; j <= Robot_basic::dof; j++)
    {
        para.clear();              // Clear all parameters for next link
        for(int i = (j-1)*NbParametresPerLink; i < j*NbParametresPerLink; i++)
          para.push_back(parameters[i]);

        jointgl[j] = Jointgl(para, position, true);
        linkgl[j] = Linkgl(para, position, jointgl[j].radius, true);
    }
}

Robotgl::Robotgl(const string & filename, const string & robotName,
                 const ColumnVector & position_) : Robot(filename, robotName),
                                                   Robotgl_basic()
{
    Config robData(true);
    ifstream inconffile(filename.c_str(), std::ios::in);
    if (robData.read_conf(inconffile))
    {
       cerr << "Robotgl::Robotgl: unable to read input config file." << endl;
    }

    bool Stl;
    string robotName_link_stl, stl_file;
    robData.select(robotName, "Stl", Stl);

    std::vector<double> parameters;
    for(int i = 1; i <= Robot::dof; i++)
    {
        parameters.push_back(links[i].get_joint_type());
        parameters.push_back(links[i].get_theta());    
        parameters.push_back(links[i].get_alpha());    
        parameters.push_back(links[i].get_a());      
        parameters.push_back(links[i].get_d());        
        parameters.push_back(0.05);                // link radius
    }

    position = position_;
    pos = ColumnVector(3); pos = 0;

    Robotgl_basic::set_dof(Robot::dof);

    double trans_x, trans_y, trans_z, rot_x, rot_y, rot_z, scal_x, scal_y, scal_z;
    float red, green, blue;
    std::vector<double> para;
    for(int j = 1; j <= Robot_basic::dof; j++)
    {
        para.clear();              // Clear all parameters for next link
        for(int i = (j-1)*NbParametresPerLink; i < j*NbParametresPerLink; i++)
          para.push_back(parameters[i]);

        if(Stl)
        {
            ostringstream ostr;
            ostr << robotName << "_LINK" << j << "_STL";
            robotName_link_stl = ostr.str();
            robData.select(robotName_link_stl, "joint_file", stl_file);
            robData.select(robotName_link_stl, "joint_translation_x", trans_x);
            robData.select(robotName_link_stl, "joint_translation_y", trans_y);
            robData.select(robotName_link_stl, "joint_translation_z", trans_z);
            robData.select(robotName_link_stl, "joint_rotation_x", rot_x);
            robData.select(robotName_link_stl, "joint_rotation_y", rot_y);
            robData.select(robotName_link_stl, "joint_rotation_z", rot_z);
            robData.select(robotName_link_stl, "joint_scaling_factor_x", scal_x);
            robData.select(robotName_link_stl, "joint_scaling_factor_y", scal_y);
            robData.select(robotName_link_stl, "joint_scaling_factor_z", scal_z);
            robData.select(robotName_link_stl,  "joint_rgb_red", red);
            robData.select(robotName_link_stl,  "joint_rgb_green", green);
            robData.select(robotName_link_stl,  "joint_rgb_blue", blue);

            jointgl[j] = Jointgl(para, position, true, trans_x, trans_y, trans_z, rot_x, 
                                 rot_y, rot_z, scal_x, scal_y, scal_z, red,
                                 green, blue, stl_file);

            robData.select(robotName_link_stl, "link_file", stl_file);
            robData.select(robotName_link_stl, "link_translation_x", trans_x);
            robData.select(robotName_link_stl, "link_translation_y", trans_y);
            robData.select(robotName_link_stl, "link_translation_z", trans_z);
            robData.select(robotName_link_stl, "link_rotation_x", rot_x);
            robData.select(robotName_link_stl, "link_rotation_y", rot_y);
            robData.select(robotName_link_stl, "link_rotation_z", rot_z);
            robData.select(robotName_link_stl, "link_scaling_factor_x", scal_x);
            robData.select(robotName_link_stl, "link_scaling_factor_y", scal_y);
            robData.select(robotName_link_stl, "link_scaling_factor_z", scal_z);
            robData.select(robotName_link_stl,  "link_rgb_red", red);
            robData.select(robotName_link_stl,  "link_rgb_green", green);
            robData.select(robotName_link_stl,  "link_rgb_blue", blue);

            linkgl[j] = Linkgl(para, position, jointgl[j].radius, true, trans_x, trans_y,
                               trans_z, rot_x, rot_y, rot_z, scal_x, scal_y, scal_z, 
                               red, green, blue, stl_file);
        }
        else
        {
            jointgl[j] = Jointgl(para, position, true);
            linkgl[j] = Linkgl(para, position, jointgl[j].radius, true);
        }
    }
}

Robotgl::Robotgl(const Robotgl & x) : Robot(x), Robotgl_basic(x)
{
}

Robotgl & Robotgl::operator=(const Robotgl & x)
{
    Robot::operator=(x);
    Robotgl_basic::operator=(x);

    return (*this);
}

void Robotgl::draw()
{
    pos = 0.0;
    glPushMatrix();

    for(int i = 1; i <= Robot_basic::dof; i++)
    {
        GLfloat transformation [] = 
        {links[i].R(1,1), links[i].R(2,1), links[i].R(3,1), 0,
         links[i].R(1,2), links[i].R(2,2), links[i].R(3,2), 0,
         links[i].R(1,3), links[i].R(2,3), links[i].R(3,3), 0,
                       0,               0,               0, 1};

        glTranslatef(pos(1), pos(2), pos(3));       
        jointgl[i].draw();

        glMultMatrixf(transformation);
        
        linkgl[i].draw();
        
        pos = links[i].get_p();
        
// pos is the position of link[i+1] in axis i+1. 
        pos = links[i].R.t()*pos;
      }

    glTranslatef(pos(1), pos(2), pos(3));
    axis->draw();

    glPopMatrix();
}

//--------------------------- R O B O T  mDH  O P E N G L -----------------------------
mRobotgl::mRobotgl(short ndof) : mRobot(ndof), Robotgl_basic(ndof)
{
    message_object = "mRobotgl";
}

mRobotgl::mRobotgl(const Matrix & initrob, const ColumnVector & position_) : 
    mRobot(initrob), Robotgl_basic()
{
    std::vector<double> parameters;
    for(int i = 1; i < initrob.Nrows(); i++)
    {
        parameters.push_back(initrob(i,1));   // joint type
        parameters.push_back(initrob(i,2));   // DH -> theta
        parameters.push_back(initrob(i,5));   // DH -> alpha
        parameters.push_back(initrob(i+1,4)); // DH -> a
        parameters.push_back(initrob(i,3));   // DH -> d
        parameters.push_back(0.05);           // link radius
    }

    pos = ColumnVector(3); pos = 0;
    position = position_;
    message_object = "mRobotgl";

    Robotgl_basic::set_dof(mRobot::dof);

    std::vector<double> para;
    for(int j = 1; j <= mRobot::dof; j++)
    {
        para.clear();              // Clear all parameters for next link
        for(int i = (j-1)*NbParametresPerLink; i < j*NbParametresPerLink; i++)
          para.push_back(parameters[i]);

        jointgl[j] = Jointgl(para, position);
        linkgl[j] = Linkgl(para, position, jointgl[j].radius);
    }
}

mRobotgl::mRobotgl(const Matrix & initrob, const Matrix & initrobm,
                   const ColumnVector & position_) : mRobot(initrob, initrobm),
                                                     Robotgl_basic()
{
    std::vector<double> parameters;
    for(int i = 1; i < initrob.Nrows(); i++)
    {
        parameters.push_back(initrob(i,1));   // joint type
        parameters.push_back(initrob(i,2));   // DH -> theta
        parameters.push_back(initrob(i,5));   // DH -> alpha
        parameters.push_back(initrob(i+1,4)); // DH -> a
        parameters.push_back(initrob(i,3));   // DH -> d
        parameters.push_back(0.05);           // link radius
    }

    pos = ColumnVector(3); pos = 0;
    position = position_;
    message_object = "mRobotgl";

    Robotgl_basic::set_dof(mRobot::dof);

    std::vector<double> para;
    for(int j = 1; j <= mRobot::dof; j++)
    {
        para.clear();              // Clear all parameters for next link
        for(int i = (j-1)*NbParametresPerLink; i < j*NbParametresPerLink; i++)
          para.push_back(parameters[i]);

        jointgl[j] = Jointgl(para, position);
        linkgl[j] = Linkgl(para, position, jointgl[j].radius);
    }
}

mRobotgl::mRobotgl(const string & filename, const string & robotName,
                   const ColumnVector & position_) : mRobot(filename, robotName),
                                                     Robotgl_basic()
{

    Config robData(true);
    ifstream inconffile(filename.c_str(), std::ios::in);
    if (robData.read_conf(inconffile))
    {
       cerr << "mRobotgl::mRobotgl: unable to read input config file." << endl;
    }

    bool Stl;
    string robotName_link_stl, stl_file;

    robData.select(robotName, "Stl", Stl);

    std::vector<double> parameters;
    for(int i = 1; i <= mRobot::dof; i++)
    {
        parameters.push_back(links[i].get_joint_type());
        parameters.push_back(links[i].get_theta());    
        parameters.push_back(links[i].get_alpha());    
        parameters.push_back(links[i+1].get_a());      
        parameters.push_back(links[i].get_d());        
        parameters.push_back(0.05);                // link radius
    }

    pos = ColumnVector(3); pos = 0;
    position = position_;
    message_object = "mRobotgl";

    Robotgl_basic::set_dof(mRobot::dof);

    double trans_x, trans_y, trans_z, rot_x, rot_y, rot_z, scal_x, scal_y, scal_z;
    float red, green, blue;
    std::vector<double> para;
    for(int j = 1; j <= mRobot::dof; j++)
    {
        para.clear();              // Clear all parameters for next link
        for(int i = (j-1)*NbParametresPerLink; i < j*NbParametresPerLink; i++)
          para.push_back(parameters[i]);

        if(Stl)
        {
            ostringstream ostr;
            ostr << robotName << "_LINK" << j << "_STL";
            robotName_link_stl = ostr.str();
            robData.select(robotName_link_stl, "joint_file", stl_file);
            robData.select(robotName_link_stl, "joint_translation_x", trans_x);
            robData.select(robotName_link_stl, "joint_translation_y", trans_y);
            robData.select(robotName_link_stl, "joint_translation_z", trans_z);
            robData.select(robotName_link_stl, "joint_rotation_x", rot_x);
            robData.select(robotName_link_stl, "joint_rotation_y", rot_y);
            robData.select(robotName_link_stl, "joint_rotation_z", rot_z);
            robData.select(robotName_link_stl, "joint_scaling_factor_x", scal_x);
            robData.select(robotName_link_stl, "joint_scaling_factor_y", scal_y);
            robData.select(robotName_link_stl, "joint_scaling_factor_z", scal_z);
            robData.select(robotName_link_stl,  "joint_rgb_red", red);
            robData.select(robotName_link_stl,  "joint_rgb_green", green);
            robData.select(robotName_link_stl,  "joint_rgb_blue", blue);

            jointgl[j] = Jointgl(para, position, false, trans_x, trans_y, trans_z, rot_x, 
                                 rot_y, rot_z, scal_x, scal_y, scal_z, red,
                                 green, blue, stl_file);

            robData.select(robotName_link_stl, "link_file", stl_file);
            robData.select(robotName_link_stl, "link_translation_x", trans_x);
            robData.select(robotName_link_stl, "link_translation_y", trans_y);
            robData.select(robotName_link_stl, "link_translation_z", trans_z);
            robData.select(robotName_link_stl, "link_rotation_x", rot_x);
            robData.select(robotName_link_stl, "link_rotation_y", rot_y);
            robData.select(robotName_link_stl, "link_rotation_z", rot_z);
            robData.select(robotName_link_stl, "link_scaling_factor_x", scal_x);
            robData.select(robotName_link_stl, "link_scaling_factor_y", scal_y);
            robData.select(robotName_link_stl, "link_scaling_factor_z", scal_z);
            robData.select(robotName_link_stl,  "link_rgb_red", red);
            robData.select(robotName_link_stl,  "link_rgb_green", green);
            robData.select(robotName_link_stl,  "link_rgb_blue", blue);

            linkgl[j] = Linkgl(para, position, jointgl[j].radius, false, trans_x, trans_y,
                               trans_z, rot_x, rot_y, rot_z, scal_x, scal_y, scal_z, 
                               red, green, blue, stl_file);
        }
        else
        {
            jointgl[j] = Jointgl(para, position);
            linkgl[j] = Linkgl(para, position, jointgl[j].radius);
        }
    }
}

mRobotgl::mRobotgl(const mRobotgl & x): mRobot(x), Robotgl_basic(x)
{
}


mRobotgl & mRobotgl::operator=(const mRobotgl & x)
{
    mRobot::operator=(x);
    Robotgl_basic::operator=(x);

   return (*this);
}


void mRobotgl::draw()
{
    pos = 0;

    glPushMatrix();
    int i;
    ColumnVector tmpColVec(3);
    for(i = 1; i <= Robotgl_basic::dof; i++)
    {
        GLfloat transformation [] = 
        {links[i].R(1,1), links[i].R(2,1), links[i].R(3,1), 0,
         links[i].R(1,2), links[i].R(2,2), links[i].R(3,2), 0,
         links[i].R(1,3), links[i].R(2,3), links[i].R(3,3), 0,
                       0,               0,               0, 1};

        glTranslatef(pos(1), pos(2), pos(3));       
        glMultMatrixf(transformation);
        
        jointgl[i].draw();
        linkgl[i].draw();
// pos is the position of link[i+1] in axis i+1.
        tmpColVec = links[i].get_p();
        pos(1) = 0.0;
        pos(2) = tmpColVec(2);
        pos(3) = tmpColVec(3);
        
        pos = links[i].R.t()*pos;
        
        if( (i < mRobot::dof) || fix )
            tmpColVec = links[i+1].get_p();
            pos(1) = tmpColVec(1);
    } 

    glTranslatef(0.0,0.0,links[i-1].get_d()); 
    axis->draw();

    glPopMatrix();
}

Linkgl::Linkgl(bool DH_): Basic_object(), STL_obj(), DH(DH_)
{
    STL = false;
}

Linkgl::Linkgl(const std::vector<double> & parameters, const ColumnVector & pos,
               double jnt_radius, const bool DH_, 
               const double stl_translation_x, const double stl_translation_y, 
               const double stl_translation_z, const double stl_rotation_x, 
               const double stl_rotation_y, const double stl_rotation_z, 
               const double stl_scalling_x, const double stl_scalling_y,
               const double stl_scalling_z, const float rgb_red,
               const float rgb_green, const float rgb_blue, const string stl_file):
    Basic_object(), STL_obj(stl_file, stl_translation_x, stl_translation_y, 
                            stl_translation_z, stl_rotation_x, stl_rotation_y, 
                            stl_rotation_z, stl_scalling_x, stl_scalling_y,
                            stl_scalling_z, rgb_red, rgb_green, rgb_blue)
{
    DH = DH_;
    STL = stl_file != "" ? true : false;

    if (parameters.size() >= NbParametresPerLink)
    {                     
        alpha = parameters[2];            
        a = parameters[3];
        d = parameters[4];
        radius = parameters[5];
        message_object = "Linkgl";
    }
    else
      error("Linkgl constructor, parameters of wrong size");
}

Linkgl::Linkgl(const Linkgl & x): Basic_object(x), STL_obj(x)
{
    alpha = x.alpha;
    a = x.a;
    d = x.d;
    radius = x.radius;
    DH = x.DH;
    STL = x.STL;
}

Linkgl::~Linkgl()
{
  glDeleteLists(listName, 1);
}

Linkgl & Linkgl::operator=(const Linkgl & x)
{
    alpha = x.alpha;
    a = x.a;
    d = x.d;
    radius = x.radius;
    DH = x.DH;
    STL = x.STL;
    Basic_object::operator=(x);
    STL_obj::operator=(x);

    return (*this);
}

/*
Figure bellow shows how each link is composed with respect with the modified DH notation 
(Craig's notation).
                       a2
                   -----------
                   |3
                   |
             a1    |d2
         ----------|
         |2
         |
    a0   |d1
  -------|
  1

*/
void Linkgl::create_list()
{
//    if(STL_obj::get_filename() != "")
    if (STL)
    {
        listName = load_STL_TextFile();
    }
    else
    {
        Cylinder cylinder;
        listName = glGenLists(1);
        
        if (listName)
        {
            glNewList(listName, GL_COMPILE);
            
            glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, rgb_blue_gray);
            glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, rgb_blue_gray);
            
            if(DH)
            {
                if(a)
                {
                    glPushMatrix();
                    glRotatef(90.0, 0.0, 1.0, 0.0);
                    cylinder.draw(radius,a);
                    glPopMatrix();
                }               
                if(d)
                {
                    glPushMatrix();
                    glTranslatef(a, 0.0, 0);
                    glRotatef(-rad2deg(alpha), 1.0, 0.0, 0.0);
                    if (d < 0)
                        glRotatef(180, 1.0, 0.0, 0.0);
                    cylinder.draw(radius, fabs(d));
                    glPopMatrix();
                }
                if(a && d)
                {
                    glPushMatrix();
                    glTranslatef(a, 0.0, 0);
                    glRotatef(90.0, 0.0, 1.0, 0.0);
                    cylinder.draw(radius, radius);
                    glPopMatrix();
                }
            }
            else
            {
                if(a)
                {
                    glPushMatrix();
                    glRotatef(90.0, 0.0, 1.0, 0.0);
                    cylinder.draw(radius, a);
                    glPopMatrix();
                }               
                if(d)
                {
                    glPushMatrix();
                    glTranslatef(a, 0.0, 0);
                    if (d < 0)
                        glRotatef(180, 1.0, 0.0, 0.0);
                    cylinder.draw(radius, fabs(d));
                    glPopMatrix();
                }
                if(a && d)
                {
                    glPushMatrix();
                    glTranslatef(a, 0.0, 0);
                    glRotatef(90.0, 0.0, 1.0, 0.0);
                    cylinder.draw(radius, radius);
                    glPopMatrix();
                }
            }   
            glEndList();
        }
        else
            error("Unable to create list for linkgl.");
    }
}

Jointgl::Jointgl(bool DH_) : Basic_object(), STL_obj()
{
    DH = DH_;
    STL = false;
}

Jointgl::Jointgl(const std::vector<double> & parameters, const ColumnVector & pos,
                 const bool DH_, const double stl_translation_x, 
                 const double stl_translation_y, const double stl_translation_z, 
                 const double stl_rotation_x, const double stl_rotation_y,    
                 const double stl_rotation_z, const double stl_scalling_x,    
                 const double stl_scalling_y, const double stl_scalling_z, 
                 const float rgb_red, const float rgb_green,  const float rgb_blue,
                 const string stl_file):
    Basic_object(), STL_obj(stl_file, stl_translation_x, stl_translation_y, 
                            stl_translation_z, stl_rotation_x, stl_rotation_y, 
                            stl_rotation_z, stl_scalling_x, stl_scalling_y,
                            stl_scalling_z, rgb_red, rgb_green, rgb_blue)
{
    DH = DH_;
    STL = (stl_file != "" ? true : false);
    prismatic = (short)parameters[0];
    radius = parameters[5]*1.4; // joint radius is 1.4*link_radius
    hight = 2.5*radius;
    message_object = "Jointgl";
}

Jointgl::Jointgl(const Jointgl & x): Basic_object(x), STL_obj(x)
{
    prismatic = x.prismatic;
    radius = x.radius;
    hight = x.hight;
    DH = x.DH;
    STL = x.STL;
}

Jointgl::~Jointgl()
{
  glDeleteLists(listName, 1);
}

Jointgl & Jointgl::operator=(const Jointgl & x)
{
    prismatic = x.prismatic;
    radius = x.radius;
    hight = x.hight;
    DH = x.DH;
    STL = x.STL;
    Basic_object::operator=(x);
    STL_obj::operator=(x);

    return (*this);
}

void Jointgl::create_list()
{
    if (STL)
    {
        listName = load_STL_TextFile();
    }
    else
    {
        Cylinder cylinder;
        listName = glGenLists(1);

        if (listName)
        {
            glNewList(listName, GL_COMPILE);
            
            glPushMatrix();
            
            glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, rgb_blue_gray);
            glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, rgb_blue_gray);
            
            if(DH)
            {
                glRotatef(180,1,0,0);
                glTranslatef(0, 0, -radius*1.5);
                cylinder.draw(radius, hight);
            }
            else
            {
                glTranslatef(0, 0, -radius*1.5);
                cylinder.draw(radius, hight);
            }
            
            glPopMatrix();
            
            glEndList();
        }
        else
            error("Unable to create list for joint.");
    }
}

---