robodyn_mechanisms: FingerKinematics.h Source File

Go to the documentation of this file.
00001 
00007 #ifndef FINGERKINEMATICS_H
00008 #define FINGERKINEMATICS_H
00009 
00010 #include "nasa_common_logging/Logger.h"
00011 #include <eigen3/Eigen/Dense>
00012 #include <kdl/utilities/svd_eigen_HH.hpp>
00013 
00014 template<unsigned int N>
00015 class FingerKinematics
00016 {
00017     std::stringstream str;
00018 public:
00019     typedef Eigen::Matrix<double, N, 1>   JointVectorType;
00020     typedef Eigen::Matrix < double, N + 1, 1 > SliderVectorType;
00021     typedef Eigen::Matrix < double, N, N + 1 > ReferenceMatrixType;
00022     typedef Eigen::Matrix < double, N + 1, N > ReferenceMatrixTransposeType;
00023     typedef Eigen::Matrix < double, N, N + 1 > ReferenceMatrixTransposeInverseType;
00024     typedef Eigen::Matrix<double, N, N>   JacobianType;
00025 
00026     FingerKinematics();
00027     virtual ~FingerKinematics() {}
00028 
00029     void setReferenceMatrix(const ReferenceMatrixType& refMatrix_in);
00030 
00031     void forward(const SliderVectorType& sliderPositions, JointVectorType& jointPositions);
00032     void inverse(const JointVectorType& jointPositions, SliderVectorType& sliderPositions);
00033     void projectToRangeSpace(const SliderVectorType& sliderPositions_in, SliderVectorType& sliderPositions_out);
00034     void tensionToJointTorque(const SliderVectorType& actualTensions, JointVectorType& jointTorques);
00035     double calculateInternalTension(const SliderVectorType& actualTensions);
00036     double findDesiredInternalTension(const double& fmin, const double& fmax, JointVectorType& jointTorques);
00037     void jointSpaceTorqueFeedback(const JointVectorType& torqueError, const JointVectorType& jointKp, const double& tensionError, const double& tensionKp, SliderVectorType& desiredSliders);
00038 
00039     void calcJacobian(const JointVectorType& jointPos, const JointVectorType& xy, const bool isLeft, JacobianType& jacobian);
00040     void jointToCartesian(const JointVectorType& jointPositions, const bool isLeft, JointVectorType& xy);
00041     void forceToJointTorque(const JacobianType& jacobian, const JointVectorType& force, JointVectorType& jointTorque);
00042 
00043 private:
00044     ReferenceMatrixType                  referenceMatrix;
00045     ReferenceMatrixTransposeType         referenceMatrixTranspose;
00046     ReferenceMatrixTransposeInverseType  referenceMatrixTransposeInverse;
00047     SliderVectorType                     nullSpaceVector;
00048 };
00049 
00050 template<unsigned int N>
00051 FingerKinematics<N>::FingerKinematics()
00052 {
00053 }
00054 
00055 template<unsigned int N>
00056 void FingerKinematics<N>::setReferenceMatrix(const ReferenceMatrixType& refMatrix_in)
00057 {
00058     referenceMatrix = refMatrix_in;
00059     referenceMatrixTranspose = refMatrix_in.transpose();
00060 
00061     // svd
00062     VectorXd tmp(referenceMatrix.cols());
00063     VectorXd s(VectorXd::Zero(referenceMatrix.cols()));
00064     MatrixXd u(MatrixXd::Identity(referenceMatrix.rows(), referenceMatrix.cols()));
00065     MatrixXd v(MatrixXd::Identity(referenceMatrix.cols(), referenceMatrix.cols()));
00066 
00067     if (KDL::svd_eigen_HH(referenceMatrix, u, s, v, tmp, 100) < 0)
00068     {
00069         std::stringstream err;
00070         err << "SVD calculation failed" << std::endl;
00071         NasaCommonLogging::Logger::log("gov.nasa.controllers.FingerKinematics", log4cpp::Priority::ERROR, err.str());
00072         throw std::runtime_error(err.str());
00073         return;
00074     }
00075 
00076     double eps = 1.e-6;
00077 
00078     for (int i = 0; i < s.rows(); ++i)
00079     {
00080         if (s(i) > eps)
00081         {
00082             s(i) = 1.0 / s(i);
00083         }
00084         else
00085         {
00086             s(i) = 0;
00087         }
00088     }
00089 
00090     referenceMatrixTransposeInverse = (v * s.asDiagonal() * u.transpose()).transpose();
00091 
00092     nullSpaceVector = v.col(referenceMatrix.rows());
00093 
00094     if (nullSpaceVector[0] < 0)
00095         for (int i = 0; i < nullSpaceVector.rows(); ++i)
00096         {
00097             nullSpaceVector[i] *= -1;
00098         }
00099 }
00100 
00101 template<unsigned int N>
00102 void FingerKinematics<N>::forward(const SliderVectorType& sliderPositions, JointVectorType& jointPositions)
00103 {
00104     jointPositions = referenceMatrixTransposeInverse * sliderPositions;
00105 }
00106 
00107 template<unsigned int N>
00108 void FingerKinematics<N>::inverse(const JointVectorType& jointPositions, SliderVectorType& sliderPositions)
00109 {
00110     sliderPositions = referenceMatrixTranspose * jointPositions;
00111 }
00112 
00113 template<unsigned int N>
00114 void FingerKinematics<N>::projectToRangeSpace(const SliderVectorType& sliderPositions_in, SliderVectorType& sliderPositions_out)
00115 {
00116     sliderPositions_out = (referenceMatrixTranspose * referenceMatrixTransposeInverse) * sliderPositions_in;
00117 }
00118 
00119 //newly added
00120 template<unsigned int N>
00121 void FingerKinematics<N>::tensionToJointTorque(const SliderVectorType& actualTensions, JointVectorType& jointTorques)
00122 {
00123     jointTorques = -referenceMatrix * actualTensions;
00124 }
00125 
00126 template<unsigned int N>
00127 double FingerKinematics<N>::calculateInternalTension(const SliderVectorType& actualTensions)
00128 {
00129     return nullSpaceVector.transpose() * actualTensions;
00130 }
00131 
00132 template<unsigned int N>
00133 double FingerKinematics<N>::findDesiredInternalTension(const double& fmin, const double& fmax, JointVectorType& jointTorques)
00134 {
00135     double t0 = 0;
00136     double d, alpha;
00137     int l, h;
00138 
00139     //find initial internal tension value
00140     for (int i = 0; i < jointTorques.rows() + 1; ++i)
00141     {
00142         d = (fmin + (referenceMatrixTransposeInverse.col(i)).transpose() * jointTorques) / nullSpaceVector[i];
00143 
00144         if (d > t0)
00145         {
00146             t0 = d;
00147         }
00148     }
00149 
00150     //check to see if internal tensions are in range
00151     SliderVectorType fTmp = -referenceMatrixTransposeInverse.transpose() * jointTorques + nullSpaceVector * t0;
00152 
00153     if (fTmp.maxCoeff() <= fmax)
00154     {
00155         return t0;
00156     }
00157 
00158     //find index numbers for highest and lowest tensions
00159     h = -1; l = -1;
00160 
00161     for (int i = 0; i < fTmp.rows(); ++i)
00162     {
00163         if (fTmp(i) == fTmp.maxCoeff())
00164         {
00165             h = i;
00166         }
00167 
00168         if (fTmp(i) == fTmp.minCoeff())
00169         {
00170             l = i;
00171         }
00172     }
00173 
00174     if (h == -1 || l == -1)
00175     {
00176         NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandFinger", log4cpp::Priority::WARN, "Index numbers l and h were not determined, check FingerKinematics.h");
00177         return t0;
00178     }
00179 
00180     //scale torques
00181     d = -(nullSpaceVector[h] * referenceMatrixTransposeInverse.col(l) - nullSpaceVector[l] * referenceMatrixTransposeInverse.col(h)).transpose() * jointTorques;
00182     alpha = (nullSpaceVector[h] * fmin - nullSpaceVector[l] * fmax) / d;
00183     t0 = -((fmax * referenceMatrixTransposeInverse.col(l) - fmin * referenceMatrixTransposeInverse.col(h)) / d).transpose() * jointTorques;
00184     jointTorques *= alpha;
00185 
00186     //Iterate if still out of range
00187     fTmp = -referenceMatrixTransposeInverse.transpose() * jointTorques + nullSpaceVector * t0;
00188 
00189     if (fTmp.maxCoeff() <= fmax)
00190     {
00191         return t0;
00192     }
00193 
00194     //find index numbers for highest and lowest tensions
00195     h = -1; l = -1;
00196 
00197     for (int i = 0; i < fTmp.rows(); ++i)
00198     {
00199         if (fTmp(i) == fTmp.maxCoeff())
00200         {
00201             h = i;
00202         }
00203 
00204         if (fTmp(i) == fTmp.minCoeff())
00205         {
00206             l = i;
00207         }
00208     }
00209 
00210     if (h == -1 || l == -1)
00211     {
00212         NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandFinger", log4cpp::Priority::WARN, "Index numbers l and h were not determined, check FingerKinematics.h");
00213         return t0;
00214     }
00215 
00216     //scale torques
00217     d = -(nullSpaceVector[h] * referenceMatrixTransposeInverse.col(l) - nullSpaceVector[l] * referenceMatrixTransposeInverse.col(h)).transpose() * jointTorques;
00218     alpha = (nullSpaceVector[h] * fmin - nullSpaceVector[l] * fmax) / d;
00219     t0 = -((fmax * referenceMatrixTransposeInverse.col(l) - fmin * referenceMatrixTransposeInverse.col(h)) / d).transpose() * jointTorques;
00220     jointTorques *= alpha;
00221     return t0;
00222 }
00223 
00224 template<unsigned int N>
00225 void FingerKinematics<N>::jointSpaceTorqueFeedback(const JointVectorType& torqueError, const JointVectorType& jointKp, const double& tensionError, const double& tensionKp, SliderVectorType& desiredSliders)
00226 {
00227     desiredSliders = referenceMatrixTranspose * jointKp.asDiagonal() * torqueError - nullSpaceVector * tensionKp * tensionError;
00228 }
00229 
00230 //newly added for Cartesian
00231 template<unsigned int N>
00232 void FingerKinematics<N>::calcJacobian(const JointVectorType& jointPos, const JointVectorType& xy, const bool isLeft, JacobianType& jacobian)
00233 {
00234     jacobian.setIdentity();
00235 
00236     if (jointPos.rows() == 3)
00237     {
00238         double L2, L3, r34;
00239         L2 = 30.48;
00240         L3 = 24.638;
00241         r34 = 7.0 / 12.0;
00242         //define Jacobian from proximal/medial joints to x-y frame
00243         jacobian(1, 1) = -xy[2];
00244         jacobian(1, 2) = -(L2 * sin(jointPos[1] + jointPos[2]) + (1 + r34) * L3 * sin(jointPos[1] + (1 + r34) * jointPos[2]));
00245         jacobian(2, 1) = xy[1];
00246         jacobian(2, 2) = (L2 * cos(jointPos[1] + jointPos[2]) + (1 + r34) * L3 * cos(jointPos[1] + (1 + r34) * jointPos[2]));
00247     }
00248     else if (jointPos.rows() == 4)
00249     {
00250         double L0 = 11.938;
00251         double L1 = 30.734;
00252         double L2 = 39.37;
00253         double L3 = 33.02;
00254 
00255         if (isLeft)
00256         {
00257             double alpha = -0.6981317;
00258             double phi = 1.309;
00259             double tempA = L2 * cos(jointPos[2]) + L3 * cos(jointPos[2] + jointPos[3]);
00260             double tempB = L2 * sin(jointPos[2]) + L3 * sin(jointPos[2] + jointPos[3]);
00261             double tempAA = L0 + (tempA + L1) * cos(jointPos[1]) - tempB * cos(alpha) * sin(jointPos[1]);
00262             double tempBB = (tempA + L1) * sin(jointPos[1]) + tempB * cos(alpha) * cos(jointPos[1]);
00263             jacobian(0, 0) = -(cos(phi) * (tempB * cos(-jointPos[0]) * sin(alpha) - tempAA * sin(-jointPos[0])));
00264             jacobian(0, 1) = cos(jointPos[1]) * (-tempB * cos(alpha) * cos(phi) * cos(-jointPos[0]) + (tempA + L1) * sin(phi)) - sin(jointPos[1]) * ((tempA + L1) * cos(phi) * cos(-jointPos[0]) + tempB * cos(alpha) * sin(phi));
00265             jacobian(0, 2) = cos(phi) * (-tempB * cos(-jointPos[0]) * cos(jointPos[1]) + tempA * sin(alpha) * sin(-jointPos[0])) - tempB * sin(phi) * sin(jointPos[1]) + tempA * cos(alpha) * (cos(jointPos[1]) * sin(phi) - cos(phi) * cos(-jointPos[0]) * sin(jointPos[1]));
00266             jacobian(0, 3) = 0;
00267             jacobian(1, 0) = -(tempAA * cos(-jointPos[0]) + tempB * sin(alpha) * sin(-jointPos[0]));
00268             jacobian(1, 1) = -tempBB * sin(-jointPos[0]);
00269             jacobian(1, 2) = -tempA * cos(-jointPos[0]) * sin(alpha) - sin(-jointPos[0]) * (tempB * cos(jointPos[1]) + tempA * cos(alpha) * sin(jointPos[1]));
00270             jacobian(1, 3) = 0;
00271             jacobian(2, 0) = -(sin(phi) * (-tempB * cos(-jointPos[0]) * sin(alpha) + tempAA * sin(-jointPos[0])));
00272             jacobian(2, 1) = tempBB * cos(-jointPos[0]) * sin(phi) + cos(phi) * ((tempA + L1) * cos(jointPos[1]) - tempB * cos(alpha) * sin(jointPos[1]));
00273             jacobian(2, 2) = tempB * cos(-jointPos[0]) * cos(jointPos[1]) * sin(phi) - tempA * sin(alpha) * sin(phi) * sin(-jointPos[0]) - tempB * cos(phi) * sin(jointPos[1]) + tempA * cos(alpha) * (cos(phi) * cos(jointPos[1]) + cos(-jointPos[0]) * sin(phi) * sin(jointPos[1]));
00274             jacobian(2, 3) = 0;
00275         }
00276         else
00277         {
00278             double alpha = 0.6981317;
00279             double phi = -1.309;
00280             double tempA = L2 * cos(jointPos[2]) + L3 * cos(jointPos[2] + jointPos[3]);
00281             double tempB = L2 * sin(jointPos[2]) + L3 * sin(jointPos[2] + jointPos[3]);
00282             double tempAA = L0 + (tempA + L1) * cos(jointPos[1]) - tempB * cos(alpha) * sin(jointPos[1]);
00283             double tempBB = (tempA + L1) * sin(jointPos[1]) + tempB * cos(alpha) * cos(jointPos[1]);
00284             jacobian(0, 0) = cos(phi) * (tempB * cos(jointPos[0]) * sin(alpha) - tempAA * sin(jointPos[0]));
00285             jacobian(0, 1) = cos(jointPos[1]) * (-tempB * cos(alpha) * cos(phi) * cos(jointPos[0]) + (tempA + L1) * sin(phi)) - sin(jointPos[1]) * ((tempA + L1) * cos(phi) * cos(jointPos[0]) + tempB * cos(alpha) * sin(phi));
00286             jacobian(0, 2) = cos(phi) * (-tempB * cos(jointPos[0]) * cos(jointPos[1]) + tempA * sin(alpha) * sin(jointPos[0])) - tempB * sin(phi) * sin(jointPos[1]) + tempA * cos(alpha) * (cos(jointPos[1]) * sin(phi) - cos(phi) * cos(jointPos[0]) * sin(jointPos[1]));
00287             jacobian(0, 3) = 0;
00288             jacobian(1, 0) = tempAA * cos(jointPos[0]) + tempB * sin(alpha) * sin(jointPos[0]);
00289             jacobian(1, 1) = -tempBB * sin(jointPos[0]);
00290             jacobian(1, 2) = -tempA * cos(jointPos[0]) * sin(alpha) - sin(jointPos[0]) * (tempB * cos(jointPos[1]) + tempA * cos(alpha) * sin(jointPos[1]));
00291             jacobian(1, 3) = 0;
00292             jacobian(2, 0) = sin(phi) * (-tempB * cos(jointPos[0]) * sin(alpha) + tempAA * sin(jointPos[0]));
00293             jacobian(2, 1) = tempBB * cos(jointPos[0]) * sin(phi) + cos(phi) * ((tempA + L1) * cos(jointPos[1]) - tempB * cos(alpha) * sin(jointPos[1]));
00294             jacobian(2, 2) = tempB * cos(jointPos[0]) * cos(jointPos[1]) * sin(phi) - tempA * sin(alpha) * sin(phi) * sin(jointPos[0]) - tempB * cos(phi) * sin(jointPos[1]) + tempA * cos(alpha) * (cos(phi) * cos(jointPos[1]) + cos(jointPos[0]) * sin(phi) * sin(jointPos[1]));
00295             jacobian(2, 3) = 0;
00296         }
00297     }
00298 }
00299 
00300 template<unsigned int N>
00301 void FingerKinematics<N>::jointToCartesian(const JointVectorType& jointPos, const bool isLeft, JointVectorType& xy)
00302 {
00303     if (jointPos.rows() == 3)
00304     {
00305         xy[0] = jointPos[0];
00306         double L1 = 44.45;
00307         double L2 = 30.48;
00308         double L3 = 24.638;
00309         double r34 = 7.0 / 12.0;
00310         xy[1] = L1 * cos(jointPos[1]) + L2 * cos(jointPos[1] + jointPos[2]) + L3 * cos(jointPos[1] + (1 + r34) * jointPos[2]);
00311         xy[2] = L1 * sin(jointPos[1]) + L2 * sin(jointPos[1] + jointPos[2]) + L3 * sin(jointPos[1] + (1 + r34) * jointPos[2]);
00312     }
00313     else if (jointPos.rows() == 4)
00314     {
00315         double L0 = 11.938;
00316         double L1 = 30.734;
00317         double L2 = 39.37;
00318         double L3 = 33.02;
00319 
00320         if (isLeft)
00321         {
00322             double alpha = -0.6981317;
00323             double phi = 1.309;
00324             double tempA = L1 + L2 * cos(jointPos[2]) + L3 * cos(jointPos[2] + jointPos[3]);
00325             double tempB = L2 * sin(jointPos[2]) + L3 * sin(jointPos[2] + jointPos[3]);
00326             xy[0] = sin(phi) * (tempA * sin(jointPos[1]) + cos(alpha) * cos(jointPos[1]) * tempB) + cos(phi) * (sin(alpha) * sin(-jointPos[0]) * tempB + cos(-jointPos[0]) * (L0 + cos(jointPos[1]) * tempA - cos(alpha) * sin(jointPos[1]) * tempB));
00327             xy[1] = -cos(-jointPos[0]) * sin(alpha) * tempB + sin(-jointPos[0]) * (L0 + cos(jointPos[1]) * tempA - cos(alpha) * sin(jointPos[1]) * tempB);
00328             xy[2] = sin(phi) * (-sin(alpha) * sin(-jointPos[0]) * tempB - cos(-jointPos[0]) * (L0 + cos(jointPos[1]) * tempA - cos(alpha) * sin(jointPos[1]) * tempB)) + cos(phi) * (sin(jointPos[1]) * tempA + cos(alpha) * cos(jointPos[1]) * tempB);
00329             xy[3] = jointPos[3];
00330         }
00331         else
00332         {
00333             double alpha = 0.6981317;
00334             double phi = 1.309;
00335             double tempA = L1 + L2 * cos(jointPos[2]) + L3 * cos(jointPos[2] + jointPos[3]);
00336             double tempB = L2 * sin(jointPos[2]) + L3 * sin(jointPos[2] + jointPos[3]);
00337             xy[0] = sin(phi) * (tempA * sin(jointPos[1]) + cos(alpha) * cos(jointPos[1]) * tempB) + cos(phi) * (sin(alpha) * sin(-jointPos[0]) * tempB + cos(-jointPos[0]) * (L0 + cos(jointPos[1]) * tempA - cos(alpha) * sin(jointPos[1]) * tempB));
00338             xy[1] = -cos(-jointPos[0]) * sin(alpha) * tempB + sin(-jointPos[0]) * (L0 + cos(jointPos[1]) * tempA - cos(alpha) * sin(jointPos[1]) * tempB);
00339             xy[2] = sin(phi) * (-sin(alpha) * sin(-jointPos[0]) * tempB - cos(-jointPos[0]) * (L0 + cos(jointPos[1]) * tempA - cos(alpha) * sin(jointPos[1]) * tempB)) + cos(phi) * (sin(jointPos[1]) * tempA + cos(alpha) * cos(jointPos[1]) * tempB);
00340             xy[3] = jointPos[3];
00341         }
00342     }
00343 
00344 //  else
00345 //        xy = jointPos;
00346 }
00347 
00348 template<unsigned int N>
00349 void FingerKinematics<N>::forceToJointTorque(const JacobianType& jacobian, const JointVectorType& force, JointVectorType& jointTorque)
00350 {
00351     if (jointTorque.rows() >= 3)
00352     {
00353         //multiply by J^T
00354         jointTorque = jacobian.transpose() * force;
00355     }
00356 
00357 //    else
00358 //        jointTorque = force;
00359 }
00360 
00361 #endif