Main Page   Namespace List   Class Hierarchy   Compound List   File List   Namespace Members   Compound Members  

SIG_RotationalJoint.cpp

00001 #include "SIGEL_Robot/SIG_RotationalJoint.h"
00002 
00003 #include "SIGEL_Robot/SIG_Joint.h"
00004 #include "SIGEL_Robot/IFunctions.h"
00005 
00006 namespace SIGEL_Robot {
00007         SIG_RotationalJoint::SIG_RotationalJoint (SIG_Robot *par, QString n, int nr=-1)
00008                 : SIG_Joint (par, n, nr)
00009         { }
00010 
00011         SIG_RotationalJoint::SIG_RotationalJoint (SIG_Robot *par, QTextStream & tx)
00012                 : SIG_Joint (par, tx)
00013         {
00014                 leftBase = SIG_Robot::streamToVector (tx);
00015                 leftDir = SIG_Robot::streamToVector (tx);
00016                 leftHand = SIG_Robot::streamToVector (tx);
00017                 rightBase = SIG_Robot::streamToVector (tx);
00018                 rightDir = SIG_Robot::streamToVector (tx);
00019                 rightHand = SIG_Robot::streamToVector (tx);
00020                 tx >> minimum >> maximum >> initial;
00021         }
00022         
00023         SIG_RotationalJoint::~SIG_RotationalJoint (void)
00024         { }
00025 
00026         SIG_Joint::JointType SIG_RotationalJoint::getJointType (void) const
00027         {
00028                 return tRotationalJoint;
00029         }
00030         
00031         void SIG_RotationalJoint::setLeftPoints (DL_vector B, DL_vector D, DL_vector H)
00032         {
00033                 leftBase = B;
00034                 leftDir = D;
00035                 leftHand = H;
00036         }
00037         
00038         void SIG_RotationalJoint::setRightPoints (DL_vector B, DL_vector D, DL_vector H)
00039         {
00040                 rightBase = B;
00041                 rightDir = D;
00042                 rightHand = H;
00043         }
00044         
00045         void SIG_RotationalJoint::setRange (DL_Scalar mn, DL_Scalar mx, DL_Scalar ii)
00046         {
00047                 minimum = mn;
00048                 maximum = mx;
00049                 initial = ii;
00050         }
00051         
00052         DL_vector SIG_RotationalJoint::getLeftBase (void) const
00053         {
00054                 return leftBase;
00055         }
00056         
00057         DL_vector SIG_RotationalJoint::getLeftDir (void) const
00058         {
00059                 return leftDir;
00060         }
00061         
00062         DL_vector SIG_RotationalJoint::getLeftHand (void) const
00063         {
00064                 return leftHand;
00065         }
00066         
00067         DL_vector SIG_RotationalJoint::getRightBase (void) const
00068         {
00069                 return rightBase;
00070         }
00071         
00072         DL_vector SIG_RotationalJoint::getRightDir (void) const
00073         {
00074                 return rightDir;
00075         }
00076         
00077         DL_vector SIG_RotationalJoint::getRightHand (void) const
00078         {
00079                 return rightHand;
00080         }
00081         
00082         DL_Scalar SIG_RotationalJoint::getMin (void) const
00083         {
00084                 return minimum;
00085         }
00086         
00087         DL_Scalar SIG_RotationalJoint::getMax (void) const
00088         {
00089                 return maximum;
00090         }
00091         
00092         DL_Scalar SIG_RotationalJoint::getIni (void) const
00093         {
00094                 return initial;
00095         }
00096 
00097         void SIG_RotationalJoint::transformPoints
00098         (SIG_Link *side, DL_vector mov, DL_matrix rot)
00099         {
00100                 if (side == leftLink) {
00101                         tfap (mov, rot, &leftBase);
00102                         tfap (mov, rot, &leftDir);
00103                         tfap (mov, rot, &leftHand);
00104                 } else if (side == rightLink) {
00105                         tfap (mov, rot, &rightBase);
00106                         tfap (mov, rot, &rightDir);
00107                         tfap (mov, rot, &rightHand);
00108                 }
00109         }
00110         
00111         void SIG_RotationalJoint::getGeomRelation (DL_vector &t, DL_matrix &o, SIG_Link *origin)
00112         {
00113                 if (leftLink == origin) {
00114                         calculateAnyJoint
00115                                 (leftBase, leftDir, leftHand,
00116                                  rightBase, rightDir, rightHand,
00117                                  initial, 0.0,
00118                                  o, t,
00119                                  name);
00120                 } else {
00121                         calculateAnyJoint
00122                                 (rightBase, rightDir, rightHand,
00123                                  leftBase, leftDir, leftHand,
00124                                  360-initial, 0.0,
00125                                  o, t,
00126                                  name);
00127                 }
00128         }
00129 
00130         void SIG_RotationalJoint::getMDH (SIG_Link * & predecessor,
00131                                           double & a, double & alpha,
00132                                           double & d, double & theta,
00133                                           double & screwD, double & screwTheta)
00134         {
00135                 switch (mdh_predecessor_is_left) {
00136                 case 0:  predecessor = rightLink; break;
00137                 case 1:  predecessor = leftLink; break;
00138                 default: predecessor = 0;
00139                 }
00140                 a = mdh_a;
00141                 alpha = mdh_alpha;
00142                 d = mdh_d;
00143                 theta = mdh_theta;
00144                 screwD = mdh_screw_d;
00145                 screwTheta = mdh_screw_theta;
00146         }
00147 
00148         void SIG_RotationalJoint::writeToFileTransfer (QTextStream & tx)
00149         {
00150                 tx << "RotationalJoint ";
00151                 SIG_Joint::writeToFileTransfer (tx);
00152                 SIG_Robot::vectorToStream (tx, leftBase);
00153                 SIG_Robot::vectorToStream (tx, leftDir);
00154                 SIG_Robot::vectorToStream (tx, leftHand);
00155                 SIG_Robot::vectorToStream (tx, rightBase);
00156                 SIG_Robot::vectorToStream (tx, rightDir);
00157                 SIG_Robot::vectorToStream (tx, rightHand);
00158                 tx << minimum << ' ' << maximum << ' ' << initial << '\n';
00159         }
00160 }

Generated at Mon Sep 3 01:32:31 2001 for PG 368 - SIGEL by doxygen1.2.3 written by Dimitri van Heesch, © 1997-2000