00001 #include "SIGEL_Robot/SIG_CylindricalJoint.h" 00002 #include "SIGEL_Robot/IFunctions.h" 00003 00004 namespace SIGEL_Robot { 00005 SIG_CylindricalJoint::SIG_CylindricalJoint (SIG_Robot *par, QString n, int nr=-1) 00006 : SIG_Joint (par, n, nr) 00007 { } 00008 00009 SIG_CylindricalJoint::SIG_CylindricalJoint (SIG_Robot *par, QTextStream & tx) 00010 : SIG_Joint (par, tx) 00011 { 00012 leftBase = SIG_Robot::streamToVector (tx); 00013 leftDir = SIG_Robot::streamToVector (tx); 00014 leftHand = SIG_Robot::streamToVector (tx); 00015 rightBase = SIG_Robot::streamToVector (tx); 00016 rightDir = SIG_Robot::streamToVector (tx); 00017 rightHand = SIG_Robot::streamToVector (tx); 00018 tx >> rotMin >> rotMax >> rotIni 00019 >> traMin >> traMax >> traIni; 00020 } 00021 00022 SIG_CylindricalJoint::~SIG_CylindricalJoint (void) 00023 { } 00024 00025 SIG_Joint::JointType SIG_CylindricalJoint::getJointType (void) const 00026 { return tCylindricalJoint; } 00027 00028 void SIG_CylindricalJoint::setLeftPoints (DL_vector B, DL_vector D, DL_vector H) 00029 { 00030 leftBase = B; 00031 leftDir = D; 00032 leftHand = H; 00033 } 00034 00035 void SIG_CylindricalJoint::setRightPoints (DL_vector B, DL_vector D, DL_vector H) 00036 { 00037 rightBase = B; 00038 rightDir = D; 00039 rightHand = H; 00040 } 00041 00042 void SIG_CylindricalJoint::setRotationalRange (DL_Scalar mn, DL_Scalar mx, DL_Scalar ii) 00043 { 00044 rotMin = mn; 00045 rotMax = mx; 00046 rotIni = ii; 00047 } 00048 00049 void SIG_CylindricalJoint::setTranslationalRange (DL_Scalar mn, DL_Scalar mx, DL_Scalar ii) 00050 { 00051 traMin = mn; 00052 traMax = mx; 00053 traIni = ii; 00054 } 00055 00056 DL_vector SIG_CylindricalJoint::getLeftBase (void) const { return leftBase; } 00057 DL_vector SIG_CylindricalJoint::getLeftDir (void) const { return leftDir; } 00058 DL_vector SIG_CylindricalJoint::getLeftHand (void) const { return leftHand; } 00059 DL_vector SIG_CylindricalJoint::getRightBase (void) const { return rightBase; } 00060 DL_vector SIG_CylindricalJoint::getRightDir (void) const { return rightDir; } 00061 DL_vector SIG_CylindricalJoint::getRightHand (void) const { return rightHand; } 00062 DL_Scalar SIG_CylindricalJoint::getMinRot (void) const { return rotMin; } 00063 DL_Scalar SIG_CylindricalJoint::getMaxRot (void) const { return rotMax; } 00064 DL_Scalar SIG_CylindricalJoint::getIniRot (void) const { return rotIni;} 00065 DL_Scalar SIG_CylindricalJoint::getMinTrans (void) const { return traMin; } 00066 DL_Scalar SIG_CylindricalJoint::getMaxTrans (void) const { return traMax; } 00067 DL_Scalar SIG_CylindricalJoint::getIniTrans (void) const { return traIni; } 00068 00069 void SIG_CylindricalJoint::transformPoints 00070 (SIG_Link *side, DL_vector mov, DL_matrix rot) 00071 { 00072 if (side == leftLink) { 00073 tfap (mov, rot, &leftBase); 00074 tfap (mov, rot, &leftDir); 00075 tfap (mov, rot, &leftHand); 00076 } else if (side == rightLink) { 00077 tfap (mov, rot, &rightBase); 00078 tfap (mov, rot, &rightDir); 00079 tfap (mov, rot, &rightHand); 00080 } 00081 } 00082 00083 void SIG_CylindricalJoint::getGeomRelation (DL_vector &t, DL_matrix &o, SIG_Link *origin) 00084 { 00085 if (leftLink == origin) { 00086 calculateAnyJoint 00087 (leftBase, leftDir, leftHand, 00088 rightBase, rightDir, rightHand, 00089 rotIni, traIni, 00090 o, t, 00091 name); 00092 } else { 00093 calculateAnyJoint 00094 (rightBase, rightDir, rightHand, 00095 leftBase, leftDir, leftHand, 00096 360-rotIni, -traIni, 00097 o, t, 00098 name); 00099 } 00100 } 00101 00102 void SIG_CylindricalJoint::writeToFileTransfer (QTextStream & tx) 00103 { 00104 tx << "CylindricalJoint "; 00105 SIG_Joint::writeToFileTransfer (tx); 00106 SIG_Robot::vectorToStream (tx, leftBase); 00107 SIG_Robot::vectorToStream (tx, leftDir); 00108 SIG_Robot::vectorToStream (tx, leftHand); 00109 SIG_Robot::vectorToStream (tx, rightBase); 00110 SIG_Robot::vectorToStream (tx, rightDir); 00111 SIG_Robot::vectorToStream (tx, rightHand); 00112 tx << rotMin << ' ' << rotMax << ' ' << rotIni << ' ' 00113 << traMin << ' ' << traMax << ' ' << traIni << '\n'; 00114 } 00115 }