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 }