ArmWidget.cpp

Go to the documentation of this file.

/****************************************************************************
**
** Copyright (C) 2010 Nokia Corporation and/or its subsidiary(-ies).
** All rights reserved.
** Contact: Nokia Corporation (qt-info@nokia.com)
**
** This file is part of the examples of the Qt Toolkit.
**
** $QT_BEGIN_LICENSE:BSD$
** You may use this file under the terms of the BSD license as follows:
**
** "Redistribution and use in source and binary forms, with or without
** modification, are permitted provided that the following conditions are
** met:
**   * Redistributions of source code must retain the above copyright
**     notice, this list of conditions and the following disclaimer.
**   * Redistributions in binary form must reproduce the above copyright
**     notice, this list of conditions and the following disclaimer in
**     the documentation and/or other materials provided with the
**     distribution.
**   * Neither the name of Nokia Corporation and its Subsidiary(-ies) nor
**     the names of its contributors may be used to endorse or promote
**     products derived from this software without specific prior written
**     permission.
**
** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
** "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
** LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
** A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
** OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
** SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
** LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
** DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
** THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
** (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
** OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE."
** $QT_END_LICENSE$
**
****************************************************************************/

#include "ArmWidget.h"

#include <math.h>
#include <QtGui>
#include "corobot.h"

double inchesToMeters(const double inches)
//convert from inches to meters
{
    return inches * 2.54 / 100.0;
}

double PixeltoMeter(const double pixel)
//convert from pixel to meter
{
    return (pixel / PIXELS_PER_METER);
}

double MetersToPixels(const double meters)
//convert from meters to pixels
{
    return (meters * PIXELS_PER_METER);
}

//Shoulder and Elbow width constant for different arms.
//Old corobot Arm
const double COROBOT_ARM_SHOULDER_SEGMENT_METERS = inchesToMeters(6.25);
const double COROBOT_ARM_ELBOW_SEGMENT_METERS = inchesToMeters(8);

//PhantomX Pincher
const double PINCHER_ARM_SHOULDER_SEGMENT_METERS = inchesToMeters(6.25);
const double PINCHER_ARM_ELBOW_SEGMENT_METERS = inchesToMeters(8);

//PhantomX Reactor
const double REACTOR_ARM_SHOULDER_SEGMENT_METERS = inchesToMeters(6.25);
const double REACTOR_ARM_ELBOW_SEGMENT_METERS = inchesToMeters(8);

//Lynxmotion al5a
const double AL5A_ARM_SHOULDER_SEGMENT_METERS = inchesToMeters(3.75);
const double AL5A_ARM_ELBOW_SEGMENT_METERS = inchesToMeters(7.62);


double armShoulderSegmentMeters=COROBOT_ARM_SHOULDER_SEGMENT_METERS;
double armElbowSegmentMeters=COROBOT_ARM_ELBOW_SEGMENT_METERS;

//Constant related to the drawings
const int ARM_X = 120;
const int ARM_Y = 120;
const int ARM_CENTER_X = ARM_X / 2;
const int ARM_BODY_TOP = ARM_X * 7 / 10;
const int ARM_SHOULDER_X = ARM_CENTER_X - ARM_X / 7;

ArmWidget::ArmWidget(QWidget *parent)
    : QGraphicsView(parent), timerId(0)
{

    QGraphicsScene *scene = new QGraphicsScene(this);
    scene->setItemIndexMethod(QGraphicsScene::NoIndex);
    scene->setSceneRect(0, 00, 210, 110);

        shoulder = true;
        elbow = true;
    setScene(scene);
    setCacheMode(CacheBackground);
    setViewportUpdateMode(BoundingRectViewportUpdate);
    setRenderHint(QPainter::Antialiasing);
    setTransformationAnchor(AnchorUnderMouse);
    scale(qreal(1), qreal(1));
    setMinimumSize(210, 110);
    setWindowTitle(tr("Elastic Nodes"));

    joint *joint1 = new joint(this);
    joint *joint2 = new joint(this);
    joint *joint3 = new joint(this);


    scene->addItem(joint1);
    scene->addItem(joint2);
    scene->addItem(joint3);

    joint3->setFlag(joint3->ItemIsMovable);

    double x = armShoulderSegmentMeters;
    double y = armElbowSegmentMeters;

    joint1->setPos(this->sceneRect().left()+ARM_SHOULDER_X+5, this->sceneRect().bottom()-ARM_Y+ARM_BODY_TOP+5);
    joint2->setPos(this->sceneRect().left()+ARM_SHOULDER_X+5, -MetersToPixels(y)-ARM_Y+ARM_BODY_TOP+5+this->sceneRect().bottom());


    joint3->setPos(MetersToPixels(x)+this->sceneRect().left()+ARM_SHOULDER_X+5,
                   -MetersToPixels(y)-ARM_Y+ARM_BODY_TOP+5+this->sceneRect().bottom());
    end_effector = QPointF(0,0);

    QGraphicsLineItem *line = new QGraphicsLineItem(joint1->pos().x()-5,joint1->pos().y()-5,joint2->pos().x()-5,joint2->pos().y()-5);
    scene->addItem(line);
    QGraphicsLineItem *line2 = new QGraphicsLineItem(joint2->pos().x()-5,joint2->pos().y()-5,joint3->pos().x()-5,joint3->pos().y()-5);
    scene->addItem(line2);

 }
bool doArmIK(const double x, const double y, double& theta1, double& theta2)
//compute the inverse kinematics for the arm
{
    const double LOWER_LINK_LEN = armShoulderSegmentMeters;
    const double UPPER_LINK_LEN = armElbowSegmentMeters;
    const double LOWER_SQUARED = armShoulderSegmentMeters * armShoulderSegmentMeters;
    const double UPPER_SQUARED = armElbowSegmentMeters * armElbowSegmentMeters;

    double c2;                  // cosine of theta2

        c2 = ((x * x) + (y * y) - LOWER_SQUARED - UPPER_SQUARED) /
            (2 * LOWER_LINK_LEN * UPPER_LINK_LEN);
        if ((c2 < -1.0) || (c2 > 1.0))
        {
            return false;
        }

        double magS2;               // magnitude of sine of theta2 (could be + or - this).

        magS2 = sqrt(1 - c2 * c2);
        double posS2 = magS2;
        double negS2 = -magS2;
        double posTheta2,
            negTheta2;

        posTheta2 = atan2(posS2, c2);
        negTheta2 = atan2(negS2, c2);

        double posS1;               // sine of theta1

        posS1 =
            ((LOWER_LINK_LEN + UPPER_LINK_LEN * c2) * y -
             (UPPER_LINK_LEN * posS2 * x)) / (x * x + y * y);
        double posC1;

        posC1 =
            ((LOWER_LINK_LEN + UPPER_LINK_LEN * c2) * x +
             (UPPER_LINK_LEN * posS2 * y)) / (x * x + y * y);
        double posTheta1;

        posTheta1 = atan2(posS1, posC1);

        double negS1;               // sine of theta1

        negS1 =
            ((LOWER_LINK_LEN + UPPER_LINK_LEN * c2) * y -
             (UPPER_LINK_LEN * negS2 * x)) / (x * x + y * y);
        double negC1;

        negC1 =
            ((LOWER_LINK_LEN + UPPER_LINK_LEN * c2) * x +
             (UPPER_LINK_LEN * negS2 * y)) / (x * x + y * y);
        double negTheta1;

        negTheta1 = atan2(negS1, negC1);

        // bounds checking - make sure the joints are within ranges.
        if ((posTheta1 <= M_PI * 0.75) && (posTheta1 >= 0.0) && (posTheta2 <= 0.0))
        {
            theta1 = posTheta1;
            theta2 = posTheta2;
            return true;
        }

        // bounds checking - make sure the joints are within ranges.
        if ((negTheta1 <= M_PI * 0.75) && (negTheta1 >= 0.0) && (negTheta2 <= 0.0))
        {
            theta1 = negTheta1;
            theta2 = negTheta2;
            return true;
        }
        return false;
}

void ArmWidget::Corobot(bool value)
// value is true of a Corobot, false if is an Explorer, Explorer doesn't have an arm therefore the Widget is white
{
        foreach (QGraphicsItem *item, scene()->items()) {
            if(value == true){
                item->show();
            }
            else{
                item->hide();
            }
        }
}

void ArmWidget::setModel(bool arm_al5a,bool arm_pincher,bool arm_reactor,bool arm_old_corobot)
// set the number of Degree of Freedom of the arm and therefore the dimensions and if the rotation of the arm should be controled or not
{
    if (arm_old_corobot)
    {
        armShoulderSegmentMeters= COROBOT_ARM_SHOULDER_SEGMENT_METERS;
        armElbowSegmentMeters= COROBOT_ARM_ELBOW_SEGMENT_METERS;
        arm_type = OldCorobot;
    }
    else if (arm_pincher)
    {
        armShoulderSegmentMeters= PINCHER_ARM_SHOULDER_SEGMENT_METERS;
        armElbowSegmentMeters= PINCHER_ARM_ELBOW_SEGMENT_METERS;
        arm_type = Pincher;
    }
    else if (arm_reactor)
    {
        armShoulderSegmentMeters= REACTOR_ARM_SHOULDER_SEGMENT_METERS;
        armElbowSegmentMeters= REACTOR_ARM_ELBOW_SEGMENT_METERS;
        arm_type = Reactor;
    }
    else if (arm_al5a)
    {
        armShoulderSegmentMeters= AL5A_ARM_SHOULDER_SEGMENT_METERS;
        armElbowSegmentMeters= AL5A_ARM_ELBOW_SEGMENT_METERS;
        arm_type = Al5a;
    }

    printf("ARM_LENGTH %d, %d", armShoulderSegmentMeters, armShoulderSegmentMeters);

    QGraphicsScene *scene = this->scene();

    joint* joint1 = (joint*)scene->items().at(0);
    joint* joint2 = (joint*)scene->items().at(1);
    joint* joint3 = (joint*)scene->items().at(2);

    double x = armShoulderSegmentMeters;
    double y = armElbowSegmentMeters;

    joint3->setPos(MetersToPixels(x)+this->sceneRect().left()+ARM_SHOULDER_X+5,-MetersToPixels(y)-ARM_Y+ARM_BODY_TOP+5+this->sceneRect().bottom());
    joint2->setPos(this->sceneRect().left()+ARM_SHOULDER_X+5, -MetersToPixels(y)-ARM_Y+ARM_BODY_TOP+5+this->sceneRect().bottom());

    ((QGraphicsLineItem*)(scene->items().at(3)))->setLine(joint1->pos().x()-5,joint1->pos().y()-5,joint2->pos().x()-5,joint2->pos().y()-5);
    ((QGraphicsLineItem*)(scene->items().at(4)))->setLine(joint2->pos().x()-5,joint2->pos().y()-5,joint3->pos().x()-5,joint3->pos().y()-5);


}


void ArmWidget::itemMoved()
//timer starts if a movement is detected
{
    if (!timerId)
        timerId = startTimer(1000 / 25);
}

void ArmWidget::shoulder_degree(bool value)
//if value is true, the signal theta1 will be emited in degrees
{
    shoulder = value;

}

void ArmWidget::elbow_degree(bool value)
//if value is true, the signal theta2 will be emited in degrees
{
    elbow = value;

}

void ArmWidget::moveArmUp()
//moves the arm up
{
    this->scene()->items().at(2)->setPos(end_effector + QPointF(0,-5));
}

void ArmWidget::moveArmDown()
//moves the arm down
{
    this->scene()->items().at(2)->setPos(end_effector + QPointF(0,5));
}

void ArmWidget::moveArmLeft()
//moves the arm toward the robot
{
    this->scene()->items().at(2)->setPos(end_effector + QPointF(-5,0));
}

void ArmWidget::moveArmRight()
//moves the arm far from the robot
{
    this->scene()->items().at(2)->setPos(end_effector + QPointF(5,0));
}

void ArmWidget::received_pos(double x, double y)
//set the position ofthe arm. x,y if he position in meters
{

    QList<joint *> joints;
        foreach (QGraphicsItem *item, scene()->items()) {
            if (joint *j = qgraphicsitem_cast<joint *>(item))
                joints << j;
        }

    joints.at(2)->setPos(MetersToPixels(x)+this->sceneRect().left()+ARM_SHOULDER_X+5,
                -MetersToPixels(y)-ARM_Y+ARM_BODY_TOP+5+this->sceneRect().bottom());


}


void ArmWidget::timerEvent(QTimerEvent *event)
//execute this function when the timer is up.
 {
     Q_UNUSED(event);

     QList<joint *> joints;
         foreach (QGraphicsItem *item, scene()->items()) {
             if (joint *j = qgraphicsitem_cast<joint *>(item))
                 joints << j;
         }

       //  foreach (joint *j, joints)
         //   j->calculateForces();
/*
         bool itemsMoved = false;
         foreach (joint *j, joints) {
             if (j->advance())
                 itemsMoved = true;
         }

         if (!itemsMoved) {
             killTimer(timerId);
       //      timerId = 0;
         }
       else{*/

         const double LINK_1_LENGTH = MetersToPixels(armShoulderSegmentMeters);
         const double LINK_2_LENGTH = MetersToPixels(armElbowSegmentMeters);

         float x,y;

             x = PixeltoMeter(joints.at(2)->pos().x()-5-(this->sceneRect().left()+ARM_SHOULDER_X));
             y = PixeltoMeter(this->sceneRect().bottom()-ARM_Y+ARM_BODY_TOP-joints.at(2)->pos().y()+5);

         if(joints.at(2)->x()!=end_effector.x() || joints.at(2)->y()!=end_effector.y()){
             double t1,t2;
             if(doArmIK(x,y, t1, t2)){

            //emit posarm(x,y);

            double x = LINK_1_LENGTH * cos(t1);
            double y = LINK_1_LENGTH * sin(t1);

            int elbowX = ARM_SHOULDER_X + (int)x;
            int elbowY = -ARM_Y+ARM_BODY_TOP - (int)y;

            joints.at(1)->setPos(this->sceneRect().left()+elbowX+5, this->sceneRect().bottom()+elbowY+5);


            if(shoulder)
                emit theta1(t1/M_PI*180);
            else
                emit theta1(t1);
            if(elbow)
                emit theta2(t2/M_PI*180+180);
            else
                emit theta2(t2 + M_PI);

            if (arm_type == Al5a)
            {
                emit shoulderAngle_rad(180 - t1);
                emit elbowAngle_rad(180 + t2);
            }
            else
            {
                emit shoulderAngle_rad(t1);
                emit elbowAngle_rad(-t2);
            }
                 QList<QGraphicsLineItem *> lines;
                     foreach (QGraphicsItem *item, scene()->items()) {
                         if (QGraphicsLineItem *l = qgraphicsitem_cast<QGraphicsLineItem *>(item))
                             lines << l;
                     }

                         lines.at(0)->setLine(joints.at(0)->pos().x()-5,joints.at(0)->pos().y()-5,joints.at(1)->pos().x()-5,joints.at(1)->pos().y()-5);
                         lines.at(1)->setLine(joints.at(1)->pos().x()-5,joints.at(1)->pos().y()-5,joints.at(2)->pos().x()-5,joints.at(2)->pos().y()-5);

                 end_effector = joints.at(2)->pos();
                 }
             else{
                joints.at(2)->setPos(end_effector);
             }
         }



 }




 void ArmWidget::drawBackground(QPainter *painter, const QRectF &rect)
 // draw the widget background
 {
     Q_UNUSED(rect);

     // Shadow
     QRectF sceneRect = this->sceneRect();
     QRectF rightShadow(sceneRect.right(), sceneRect.top() + 5, 5, sceneRect.height());
     QRectF bottomShadow(sceneRect.left() + 5, sceneRect.bottom(), sceneRect.width(), 5);
     if (rightShadow.intersects(rect) || rightShadow.contains(rect))
         painter->fillRect(rightShadow, Qt::darkGray);
     if (bottomShadow.intersects(rect) || bottomShadow.contains(rect))
         painter->fillRect(bottomShadow, Qt::darkGray);


     // Fill
     QLinearGradient gradient(sceneRect.topLeft(), sceneRect.bottomRight());
     gradient.setColorAt(0, Qt::white);
     gradient.setColorAt(1, Qt::lightGray);
     painter->fillRect(rect.intersect(sceneRect), gradient);
     painter->setBrush(Qt::NoBrush);
     painter->drawRect(sceneRect);

     //Robot
      QRectF robot(sceneRect.left(), sceneRect.bottom()-ARM_Y+ARM_BODY_TOP, ARM_X / 2, ARM_Y - ARM_BODY_TOP);
      painter->fillRect(robot, Qt::gray);
      painter->setBrush(Qt::NoBrush);
      painter->drawRect(robot);


     // Text
     QRectF textRect(sceneRect.left() + 4, sceneRect.top() + 4,
                     sceneRect.width() - 4, sceneRect.height() - 4);

     QFont font = painter->font();
     font.setBold(true);
     font.setPointSize(14);
     painter->setFont(font);
     painter->setPen(Qt::lightGray);
     painter->setPen(Qt::black);
 }

 void ArmWidget::scaleView(qreal scaleFactor)
 //scale the view
 {
     qreal factor = transform().scale(scaleFactor, scaleFactor).mapRect(QRectF(0, 0, 1, 1)).width();
     if (factor < 0.07 || factor > 100)
         return;

     scale(scaleFactor, scaleFactor);
 }

//bool arm_reset()
//{
//}