ArmWidget.cpp
Go to the documentation of this file.
00001 /*
00002 
00003  * Copyright (c) 2009, CoroWare
00004  * All rights reserved.
00005  * 
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are met:
00008  * 
00009  *     * Redistributions of source code must retain the above copyright
00010  *       notice, this list of conditions and the following disclaimer.
00011  *     * Redistributions in binary form must reproduce the above copyright
00012  *       notice, this list of conditions and the following disclaimer in the
00013  *       documentation and/or other materials provided with the distribution.
00014  *     * Neither the name of the Willow Garage, Stanford U. nor the names of its
00015  *       contributors may be used to endorse or promote products derived from
00016  *       this software without specific prior written permission.
00017  * 
00018  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00022  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00023  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00024  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00028  * POSSIBILITY OF SUCH DAMAGE.
00029  */
00030 
00031 #include "ArmWidget.h"
00032 
00033 #include <math.h>
00034 #include <QtGui>
00035 #include "corobot.h"
00036 
00037 double inchesToMeters(const double inches)
00038 //convert from inches to meters
00039 {
00040     return inches * 2.54 / 100.0;
00041 }
00042 
00043 double PixeltoMeter(const double pixel)
00044 //convert from pixel to meter
00045 {
00046     return (pixel / PIXELS_PER_METER);
00047 }
00048 
00049 double MetersToPixels(const double meters)
00050 //convert from meters to pixels
00051 {
00052     return (meters * PIXELS_PER_METER);
00053 }
00054 
00055 //Shoulder and Elbow width constant for different arms. Some constants are not known so they are given a false value
00056 //Old corobot Arm
00057 const double COROBOT_ARM_SHOULDER_SEGMENT_METERS = inchesToMeters(6.25);
00058 const double COROBOT_ARM_ELBOW_SEGMENT_METERS = inchesToMeters(8);
00059 
00060 //PhantomX Pincher
00061 const double PINCHER_ARM_SHOULDER_SEGMENT_METERS = inchesToMeters(6.25);
00062 const double PINCHER_ARM_ELBOW_SEGMENT_METERS = inchesToMeters(8);
00063 
00064 //PhantomX Reactor
00065 const double REACTOR_ARM_SHOULDER_SEGMENT_METERS = inchesToMeters(6.25);
00066 const double REACTOR_ARM_ELBOW_SEGMENT_METERS = inchesToMeters(8);
00067 
00068 //Lynxmotion al5a
00069 const double AL5A_ARM_SHOULDER_SEGMENT_METERS = inchesToMeters(3.75);
00070 const double AL5A_ARM_ELBOW_SEGMENT_METERS = inchesToMeters(7.62);
00071 
00072 
00073 double armShoulderSegmentMeters=COROBOT_ARM_SHOULDER_SEGMENT_METERS;
00074 double armElbowSegmentMeters=COROBOT_ARM_ELBOW_SEGMENT_METERS;
00075 
00076 //Constant related to the position of the arm in the widget
00077 const int ARM_X = 120;
00078 const int ARM_Y = 120;
00079 const int ARM_CENTER_X = ARM_X / 2;
00080 const int ARM_BODY_TOP = ARM_X * 7 / 10;
00081 const int ARM_SHOULDER_X = ARM_CENTER_X - ARM_X / 7;
00082 
00083 
00084 ArmWidget::ArmWidget(QWidget *parent)
00085     : QGraphicsView(parent), timerId(0)
00086 {
00087 
00088     QGraphicsScene *scene = new QGraphicsScene(this);
00089     scene->setItemIndexMethod(QGraphicsScene::NoIndex);
00090     scene->setSceneRect(0, 00, 210, 110);
00091 
00092         shoulder = true;
00093         elbow = true;
00094     setScene(scene);
00095     setCacheMode(CacheBackground);
00096     setViewportUpdateMode(BoundingRectViewportUpdate);
00097     setRenderHint(QPainter::Antialiasing);
00098     setTransformationAnchor(AnchorUnderMouse);
00099     scale(qreal(1), qreal(1));
00100     setMinimumSize(210, 110);
00101     setWindowTitle(tr("Elastic Nodes"));
00102 
00103     joint *joint1 = new joint(this); // shoulder joint, fixed
00104     joint *joint2 = new joint(this); // elbow joint, not fixed but not moveable directly
00105     joint *joint3 = new joint(this); // gripper, moveable by the user
00106 
00107 
00108     scene->addItem(joint1);
00109     scene->addItem(joint2);
00110     scene->addItem(joint3);
00111 
00112     joint3->setFlag(joint3->ItemIsMovable);
00113 
00114     double x = armShoulderSegmentMeters;
00115     double y = armElbowSegmentMeters;
00116 
00117     joint1->setPos(this->sceneRect().left()+ARM_SHOULDER_X+5, this->sceneRect().bottom()-ARM_Y+ARM_BODY_TOP+5);
00118     joint2->setPos(this->sceneRect().left()+ARM_SHOULDER_X+5, -MetersToPixels(y)-ARM_Y+ARM_BODY_TOP+5+this->sceneRect().bottom());
00119 
00120 
00121     joint3->setPos(MetersToPixels(x)+this->sceneRect().left()+ARM_SHOULDER_X+5,
00122                    -MetersToPixels(y)-ARM_Y+ARM_BODY_TOP+5+this->sceneRect().bottom());
00123     end_effector = QPointF(0,0);
00124 
00125     QGraphicsLineItem *line = new QGraphicsLineItem(joint1->pos().x()-5,joint1->pos().y()-5,joint2->pos().x()-5,joint2->pos().y()-5); //line between shoulder and elbow
00126     scene->addItem(line);
00127     QGraphicsLineItem *line2 = new QGraphicsLineItem(joint2->pos().x()-5,joint2->pos().y()-5,joint3->pos().x()-5,joint3->pos().y()-5); // line between the elbow and the gripper
00128     scene->addItem(line2);
00129 
00130  }
00138 bool doArmIK(const double x, const double y, double& theta1, double& theta2)
00139 //compute the inverse kinematics for the arm
00140 {
00141     const double LOWER_LINK_LEN = armShoulderSegmentMeters;
00142     const double UPPER_LINK_LEN = armElbowSegmentMeters;
00143     const double LOWER_SQUARED = armShoulderSegmentMeters * armShoulderSegmentMeters;
00144     const double UPPER_SQUARED = armElbowSegmentMeters * armElbowSegmentMeters;
00145 
00146     double c2;                  // cosine of theta2
00147 
00148         c2 = ((x * x) + (y * y) - LOWER_SQUARED - UPPER_SQUARED) /
00149             (2 * LOWER_LINK_LEN * UPPER_LINK_LEN);
00150         if ((c2 < -1.0) || (c2 > 1.0))
00151         {
00152             return false;
00153         }
00154 
00155         double magS2;               // magnitude of sine of theta2 (could be + or - this).
00156 
00157         magS2 = sqrt(1 - c2 * c2);
00158         double posS2 = magS2;
00159         double negS2 = -magS2;
00160         double posTheta2,
00161             negTheta2;
00162 
00163         posTheta2 = atan2(posS2, c2);
00164         negTheta2 = atan2(negS2, c2);
00165 
00166         double posS1;               // sine of theta1
00167 
00168         posS1 =
00169             ((LOWER_LINK_LEN + UPPER_LINK_LEN * c2) * y -
00170              (UPPER_LINK_LEN * posS2 * x)) / (x * x + y * y);
00171         double posC1;
00172 
00173         posC1 =
00174             ((LOWER_LINK_LEN + UPPER_LINK_LEN * c2) * x +
00175              (UPPER_LINK_LEN * posS2 * y)) / (x * x + y * y);
00176         double posTheta1;
00177 
00178         posTheta1 = atan2(posS1, posC1);
00179 
00180         double negS1;               // sine of theta1
00181 
00182         negS1 =
00183             ((LOWER_LINK_LEN + UPPER_LINK_LEN * c2) * y -
00184              (UPPER_LINK_LEN * negS2 * x)) / (x * x + y * y);
00185         double negC1;
00186 
00187         negC1 =
00188             ((LOWER_LINK_LEN + UPPER_LINK_LEN * c2) * x +
00189              (UPPER_LINK_LEN * negS2 * y)) / (x * x + y * y);
00190         double negTheta1;
00191 
00192         negTheta1 = atan2(negS1, negC1);
00193 
00194         // bounds checking - make sure the joints are within ranges.
00195         if ((posTheta1 <= M_PI * 0.75) && (posTheta1 >= 0.0) && (posTheta2 <= 0.0))
00196         {
00197             theta1 = posTheta1;
00198             theta2 = posTheta2;
00199             return true;
00200         }
00201 
00202         // bounds checking - make sure the joints are within ranges.
00203         if ((negTheta1 <= M_PI * 0.75) && (negTheta1 >= 0.0) && (negTheta2 <= 0.0))
00204         {
00205             theta1 = negTheta1;
00206             theta2 = negTheta2;
00207             return true;
00208         }
00209         return false;
00210 }
00211 
00212 void ArmWidget::Corobot(bool value)
00213 // value is true of a Corobot, false if is an Explorer, Explorer doesn't have an arm therefore the Widget is white
00214 {
00215         foreach (QGraphicsItem *item, scene()->items()) {
00216             if(value == true){
00217                 item->show();
00218             }
00219             else{
00220                 item->hide();
00221             }
00222         }
00223 }
00224 
00225 void ArmWidget::setModel(bool arm_al5a,bool arm_pincher,bool arm_reactor,bool arm_old_corobot)
00226 // set the number of Degree of Freedom of the arm and therefore the dimensions.
00227 {
00228     if (arm_old_corobot)
00229     {
00230         armShoulderSegmentMeters= COROBOT_ARM_SHOULDER_SEGMENT_METERS;
00231         armElbowSegmentMeters= COROBOT_ARM_ELBOW_SEGMENT_METERS;
00232         arm_type = OldCorobot;
00233     }
00234     else if (arm_pincher)
00235     {
00236         armShoulderSegmentMeters= PINCHER_ARM_SHOULDER_SEGMENT_METERS;
00237         armElbowSegmentMeters= PINCHER_ARM_ELBOW_SEGMENT_METERS;
00238         arm_type = Pincher;
00239     }
00240     else if (arm_reactor)
00241     {
00242         armShoulderSegmentMeters= REACTOR_ARM_SHOULDER_SEGMENT_METERS;
00243         armElbowSegmentMeters= REACTOR_ARM_ELBOW_SEGMENT_METERS;
00244         arm_type = Reactor;
00245     }
00246     else if (arm_al5a)
00247     {
00248         armShoulderSegmentMeters= AL5A_ARM_SHOULDER_SEGMENT_METERS;
00249         armElbowSegmentMeters= AL5A_ARM_ELBOW_SEGMENT_METERS;
00250         arm_type = Al5a;
00251     }
00252 
00253     printf("ARM_LENGTH %f, %f", armShoulderSegmentMeters, armShoulderSegmentMeters);
00254 
00255     QGraphicsScene *scene = this->scene();
00256 
00257     joint* joint1 = (joint*)scene->items().at(0);
00258     joint* joint2 = (joint*)scene->items().at(1);
00259     joint* joint3 = (joint*)scene->items().at(2);
00260 
00261     double x = armShoulderSegmentMeters;
00262     double y = armElbowSegmentMeters;
00263 
00264     joint3->setPos(MetersToPixels(x)+this->sceneRect().left()+ARM_SHOULDER_X+5,-MetersToPixels(y)-ARM_Y+ARM_BODY_TOP+5+this->sceneRect().bottom());
00265     joint2->setPos(this->sceneRect().left()+ARM_SHOULDER_X+5, -MetersToPixels(y)-ARM_Y+ARM_BODY_TOP+5+this->sceneRect().bottom());
00266 
00267     ((QGraphicsLineItem*)(scene->items().at(3)))->setLine(joint1->pos().x()-5,joint1->pos().y()-5,joint2->pos().x()-5,joint2->pos().y()-5);
00268     ((QGraphicsLineItem*)(scene->items().at(4)))->setLine(joint2->pos().x()-5,joint2->pos().y()-5,joint3->pos().x()-5,joint3->pos().y()-5);
00269 
00270 
00271 }
00272 
00273 
00274 void ArmWidget::itemMoved()
00275 //timer starts if a movement is detected
00276 {
00277     if (!timerId)
00278         timerId = startTimer(1000 / 25);
00279 }
00280 
00281 void ArmWidget::shoulder_degree(bool value)
00282 //if value is true, the signal theta1 will be emited in degrees
00283 {
00284     shoulder = value;
00285 
00286 }
00287 
00288 void ArmWidget::elbow_degree(bool value)
00289 //if value is true, the signal theta2 will be emited in degrees
00290 {
00291     elbow = value;
00292 
00293 }
00294 
00295 void ArmWidget::moveArmUp()
00296 //moves the arm up
00297 {
00298     this->scene()->items().at(2)->setPos(end_effector + QPointF(0,-5));
00299 }
00300 
00301 void ArmWidget::moveArmDown()
00302 //moves the arm down
00303 {
00304     this->scene()->items().at(2)->setPos(end_effector + QPointF(0,5));
00305 }
00306 
00307 void ArmWidget::moveArmLeft()
00308 //moves the arm toward the robot
00309 {
00310     this->scene()->items().at(2)->setPos(end_effector + QPointF(-5,0));
00311 }
00312 
00313 void ArmWidget::moveArmRight()
00314 //moves the arm far from the robot
00315 {
00316     this->scene()->items().at(2)->setPos(end_effector + QPointF(5,0));
00317 }
00318 
00319 void ArmWidget::received_pos(double x, double y)
00320 //set the position ofthe arm. x,y if he position in meters
00321 {
00322 
00323     QList<joint *> joints;
00324         foreach (QGraphicsItem *item, scene()->items()) {
00325             if (joint *j = qgraphicsitem_cast<joint *>(item))
00326                 joints << j;
00327         }
00328 
00329     joints.at(2)->setPos(MetersToPixels(x)+this->sceneRect().left()+ARM_SHOULDER_X+5,
00330                 -MetersToPixels(y)-ARM_Y+ARM_BODY_TOP+5+this->sceneRect().bottom());
00331 
00332 
00333 }
00334 
00335 
00336 void ArmWidget::timerEvent(QTimerEvent *event)
00337 //execute this function when the timer is up. the timer prevents impossible movements to the arm.
00338  {
00339      Q_UNUSED(event);
00340 
00341         // get all the joints
00342      QList<joint *> joints;
00343          foreach (QGraphicsItem *item, scene()->items()) {
00344              if (joint *j = qgraphicsitem_cast<joint *>(item))
00345                  joints << j;
00346          }
00347 
00348 
00349          const double LINK_1_LENGTH = MetersToPixels(armShoulderSegmentMeters);
00350          const double LINK_2_LENGTH = MetersToPixels(armElbowSegmentMeters);
00351 
00352          float x,y;
00353 
00354              x = PixeltoMeter(joints.at(2)->pos().x()-5-(this->sceneRect().left()+ARM_SHOULDER_X));
00355              y = PixeltoMeter(this->sceneRect().bottom()-ARM_Y+ARM_BODY_TOP-joints.at(2)->pos().y()+5);
00356 
00357          if(joints.at(2)->x()!=end_effector.x() || joints.at(2)->y()!=end_effector.y()){ // if the gripper moved
00358              double t1,t2;
00359              if(doArmIK(x,y, t1, t2)){ // do the inverse kinematic with the new gripper position
00360 
00361             double x = LINK_1_LENGTH * cos(t1);
00362             double y = LINK_1_LENGTH * sin(t1);
00363 
00364             int elbowX = ARM_SHOULDER_X + (int)x;
00365             int elbowY = -ARM_Y+ARM_BODY_TOP - (int)y;
00366 
00367             joints.at(1)->setPos(this->sceneRect().left()+elbowX+5, this->sceneRect().bottom()+elbowY+5); // set the position to the elbow
00368 
00369 
00370             if(shoulder)
00371                 emit theta1(t1/M_PI*180);
00372             else
00373                 emit theta1(t1);
00374             if(elbow)
00375                 emit theta2(t2/M_PI*180+180);
00376             else
00377                 emit theta2(t2 + M_PI);
00378 
00379             if (arm_type == Al5a)
00380             {
00381                 emit shoulderAngle_rad(180 - t1);
00382                 emit elbowAngle_rad(180 + t2);
00383             }
00384             else
00385             {
00386                 emit shoulderAngle_rad(t1);
00387                 emit elbowAngle_rad(-t2);
00388             }
00389         
00390                 // Draw the arm lines 
00391                  QList<QGraphicsLineItem *> lines;
00392                      foreach (QGraphicsItem *item, scene()->items()) {
00393                          if (QGraphicsLineItem *l = qgraphicsitem_cast<QGraphicsLineItem *>(item))
00394                              lines << l;
00395                      }
00396 
00397                          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);
00398                          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);
00399 
00400                  end_effector = joints.at(2)->pos();
00401                  }
00402              else{
00403                 joints.at(2)->setPos(end_effector);
00404              }
00405          }
00406 
00407 
00408 
00409  }
00410 
00411 
00412 
00413 
00414  void ArmWidget::drawBackground(QPainter *painter, const QRectF &rect)
00415  // draw the widget background
00416  {
00417      Q_UNUSED(rect);
00418 
00419      // Shadow
00420      QRectF sceneRect = this->sceneRect();
00421      QRectF rightShadow(sceneRect.right(), sceneRect.top() + 5, 5, sceneRect.height());
00422      QRectF bottomShadow(sceneRect.left() + 5, sceneRect.bottom(), sceneRect.width(), 5);
00423      if (rightShadow.intersects(rect) || rightShadow.contains(rect))
00424          painter->fillRect(rightShadow, Qt::darkGray);
00425      if (bottomShadow.intersects(rect) || bottomShadow.contains(rect))
00426          painter->fillRect(bottomShadow, Qt::darkGray);
00427 
00428 
00429      // Fill
00430      QLinearGradient gradient(sceneRect.topLeft(), sceneRect.bottomRight());
00431      gradient.setColorAt(0, Qt::white);
00432      gradient.setColorAt(1, Qt::lightGray);
00433      painter->fillRect(rect.intersect(sceneRect), gradient);
00434      painter->setBrush(Qt::NoBrush);
00435      painter->drawRect(sceneRect);
00436 
00437      //Robot
00438       QRectF robot(sceneRect.left(), sceneRect.bottom()-ARM_Y+ARM_BODY_TOP, ARM_X / 2, ARM_Y - ARM_BODY_TOP);
00439       painter->fillRect(robot, Qt::gray);
00440       painter->setBrush(Qt::NoBrush);
00441       painter->drawRect(robot);
00442 
00443 
00444      // Text
00445      QRectF textRect(sceneRect.left() + 4, sceneRect.top() + 4,
00446                      sceneRect.width() - 4, sceneRect.height() - 4);
00447 
00448      QFont font = painter->font();
00449      font.setBold(true);
00450      font.setPointSize(14);
00451      painter->setFont(font);
00452      painter->setPen(Qt::lightGray);
00453      painter->setPen(Qt::black);
00454  }


corobot_teleop
Author(s):
autogenerated on Sun Oct 5 2014 23:18:16