00001 #include "robodyn_mechanisms/GripperKinematics.h" 00002 00003 using namespace std; 00004 using namespace log4cpp; 00005 00009 GripperKinematics::GripperKinematics(const double l1, const double l2, const double l3, const double l4, const double minSliderPosition, const double ballScrewEfficiency, 00010 const double ballScrewMotionRatio, const double jawAngleZeroAdj1, const double jawAngleZeroAdj2, const double gripperLength, 00011 const double motorTorqueConstant, const double overCenterAngle, const double overCenterStatusBuffer, double coulombFrictionCurrentOffset, 00012 const double closeToZero, const bool beSilent) 00013 { 00014 setParameters(l1, l2, l3, l4, minSliderPosition, ballScrewEfficiency, 00015 ballScrewMotionRatio, jawAngleZeroAdj1, jawAngleZeroAdj2, gripperLength, 00016 motorTorqueConstant, overCenterAngle, overCenterStatusBuffer, coulombFrictionCurrentOffset, 00017 closeToZero, beSilent); 00018 } 00019 00020 void GripperKinematics::setParameters(const double l1, const double l2, const double l3, const double l4, const double minSliderPosition, const double ballScrewEfficiency, 00021 const double ballScrewMotionRatio, const double jawAngleZeroAdj1, const double jawAngleZeroAdj2, const double gripperLength, 00022 const double motorTorqueConstant, const double overCenterAngle, const double overCenterStatusBuffer, double coulombFrictionCurrentOffset, 00023 const double closeToZero, const bool beSilent) 00024 { 00025 L1 = l1 * 0.0254; 00026 L2 = l2 * 0.0254; 00027 L3 = l3 * 0.0254; 00028 L4 = l4 * 0.0254; 00029 MIN_SLIDER_POSITION = minSliderPosition * 0.0254; 00030 BALL_SCREW_EFFICIENCY = ballScrewEfficiency; 00031 BALL_SCREW_MOTION_RATIO = ballScrewMotionRatio / (2.0 * boost::math::constants::pi<double>()); 00032 BALL_SCREW_FORCE_RATIO = BALL_SCREW_EFFICIENCY / BALL_SCREW_MOTION_RATIO; 00033 JAW_ANGLE_ZERO_ADJ = std::atan2(jawAngleZeroAdj1, jawAngleZeroAdj2); 00034 GRIPPER_LENGTH = gripperLength * 0.0254; 00035 MOTOR_TORQUE_CONSTANT = motorTorqueConstant; 00036 OVER_CENTER_ANGLE = overCenterAngle; 00037 OVER_CENTER_STATUS_BUFFER = overCenterStatusBuffer; 00038 COULOMB_FRICTION_CURRENT_OFFSET = coulombFrictionCurrentOffset; 00039 CLOSE_TO_ZERO = closeToZero; 00040 BE_SILENT = beSilent; 00041 } 00042 00046 double GripperKinematics::getEncoderAngle(const double& jawAngle) 00047 { 00048 return -((92.7711 * pow(-jawAngle, 3)) + (121.2354 * pow(-jawAngle, 2)) + (80.4027 * -jawAngle) - 17.229); 00049 } 00050 00054 double GripperKinematics::getAngleOffSingular(const double& encoderAngle, const double& jawAngle) 00055 { 00056 double x = (-encoderAngle * BALL_SCREW_MOTION_RATIO) + MIN_SLIDER_POSITION; 00057 double d = pow((pow(x, 2) + pow(L4, 2)), 0.5); 00058 00059 if (d == 0) 00060 { 00061 if (not BE_SILENT) 00062 { 00063 BE_SILENT = true; 00064 stringstream err; 00065 err << "getAngleOffSingular() - d is zero"; 00066 NasaCommonLogging::Logger::log("gov.nasa.robodyn.GripperKinematics", Priority::ERROR, err.str()); 00067 throw runtime_error(err.str()); 00068 } 00069 else 00070 { 00071 return -1; 00072 } 00073 } 00074 00075 double sliderAngle = atan2(x, L4); 00076 00077 if (boost::math::isnan(sliderAngle)) 00078 { 00079 if (not BE_SILENT) 00080 { 00081 BE_SILENT = true; 00082 stringstream err; 00083 err << "getAngleOffSingular() - sliderAngle is NaN - atan2(" << x << ", " << L4 << ")"; 00084 NasaCommonLogging::Logger::log("gov.nasa.robodyn.GripperKinematics", Priority::ERROR, err.str()); 00085 throw runtime_error(err.str()); 00086 } 00087 else 00088 { 00089 return -1; 00090 } 00091 } 00092 00093 double theta2 = boost::math::constants::pi<double>() - (JAW_ANGLE_ZERO_ADJ + sliderAngle + jawAngle); 00094 double a_2 = pow(d, 2) + pow(L3, 2) - (2 * d * L3 * cos(theta2)); 00095 00096 if (a_2 <= 0) 00097 { 00098 if (not BE_SILENT) 00099 { 00100 BE_SILENT = true; 00101 stringstream err; 00102 err << "getAngleOffSingular() - a_2 is non-positive"; 00103 NasaCommonLogging::Logger::log("gov.nasa.robodyn.GripperKinematics", Priority::ERROR, err.str()); 00104 throw runtime_error(err.str()); 00105 } 00106 else 00107 { 00108 return -1; 00109 } 00110 } 00111 00112 double a = pow(a_2, 0.5); 00113 double pre_alpha = (pow(L3, 2) - pow(d, 2) - a_2) / (-2 * d * a); 00114 00115 if (pre_alpha > 1.0) 00116 { 00117 pre_alpha = 1.0; 00118 } 00119 else if (pre_alpha < -1.0) 00120 { 00121 pre_alpha = -1.0; 00122 } 00123 00124 double alpha = acos(pre_alpha); 00125 00126 if (boost::math::isnan(alpha)) 00127 { 00128 if (not BE_SILENT) 00129 { 00130 BE_SILENT = true; 00131 stringstream err; 00132 err << "getAngleOffSingular() - alpha is NaN - acos(" << pre_alpha << ")"; 00133 NasaCommonLogging::Logger::log("gov.nasa.robodyn.GripperKinematics", Priority::ERROR, err.str()); 00134 throw runtime_error(err.str()); 00135 } 00136 else 00137 { 00138 return -1; 00139 } 00140 } 00141 00142 if (L1 == 0) 00143 { 00144 if (not BE_SILENT) 00145 { 00146 BE_SILENT = true; 00147 stringstream err; 00148 err << "getAngleOffSingular() - L1 is zero"; 00149 NasaCommonLogging::Logger::log("gov.nasa.robodyn.GripperKinematics", Priority::ERROR, err.str()); 00150 throw runtime_error(err.str()); 00151 } 00152 else 00153 { 00154 return -1; 00155 } 00156 } 00157 00158 double pre_beta = (pow(L2, 2) - pow(L1, 2) - a_2) / (-2 * L1 * a); 00159 00160 if (pre_beta > 1.0) 00161 { 00162 pre_beta = 1.0; 00163 } 00164 else if (pre_beta < -1.0) 00165 { 00166 pre_beta = -1.0; 00167 } 00168 00169 double beta = acos(pre_beta); 00170 00171 if (boost::math::isnan(beta)) 00172 { 00173 if (not BE_SILENT) 00174 { 00175 BE_SILENT = true; 00176 stringstream err; 00177 err << "getAngleOffSingular() - beta is NaN - acos(" << pre_beta << ")"; 00178 NasaCommonLogging::Logger::log("gov.nasa.robodyn.GripperKinematics", Priority::ERROR, err.str()); 00179 throw runtime_error(err.str()); 00180 } 00181 else 00182 { 00183 return -1; 00184 } 00185 } 00186 00187 double theta1 = alpha + beta; 00188 00189 if (L2 == 0) 00190 { 00191 if (not BE_SILENT) 00192 { 00193 BE_SILENT = true; 00194 stringstream err; 00195 err << "getAngleOffSingular() - L2 is zero"; 00196 NasaCommonLogging::Logger::log("gov.nasa.robodyn.GripperKinematics", Priority::ERROR, err.str()); 00197 throw runtime_error(err.str()); 00198 } 00199 else 00200 { 00201 return -1; 00202 } 00203 } 00204 00205 double pre_gamma = (pow(d, 2) + pow(L3, 2) - pow(L1, 2) - pow(L2, 2) - (2 * d * L3 * cos(theta2))) / (-2 * L1 * L2); 00206 00207 if (pre_gamma > 1.0) 00208 { 00209 pre_gamma = 1.0; 00210 } 00211 else if (pre_gamma < -1.0) 00212 { 00213 pre_gamma = -1.0; 00214 } 00215 00216 double gamma = acos(pre_gamma); 00217 00218 if (boost::math::isnan(gamma)) 00219 { 00220 if (not BE_SILENT) 00221 { 00222 BE_SILENT = true; 00223 stringstream err; 00224 err << "getAngleOffSingular() - gamma is NaN - acos(" << pre_gamma << ")"; 00225 NasaCommonLogging::Logger::log("gov.nasa.robodyn.GripperKinematics", Priority::ERROR, err.str()); 00226 throw runtime_error(err.str()); 00227 } 00228 else 00229 { 00230 return -1; 00231 } 00232 } 00233 00234 double theta3 = theta1 - sliderAngle; 00235 00236 BE_SILENT = false; 00237 return theta3; 00238 } 00239 00243 bool GripperKinematics::isOverCenter(const double& encoderAngle, const double& jawAngle) 00244 { 00245 return (getAngleOffSingular(encoderAngle, jawAngle) < OVER_CENTER_ANGLE); 00246 } 00247 00251 bool GripperKinematics::isOverCenterStatus(const double& encoderAngle, const double& jawAngle) 00252 { 00253 return (getAngleOffSingular(encoderAngle, jawAngle) < (OVER_CENTER_ANGLE + OVER_CENTER_STATUS_BUFFER)); 00254 } 00255 00259 double GripperKinematics::getMotorCurrentLimit(const double& encoderAngle, const double& jawAngle, const double& forceLimit) 00260 { 00261 double x = (-encoderAngle * BALL_SCREW_MOTION_RATIO) + MIN_SLIDER_POSITION; 00262 double d = pow((pow(x, 2) + pow(L4, 2)), 0.5); 00263 00264 if (d == 0) 00265 { 00266 if (not BE_SILENT) 00267 { 00268 BE_SILENT = true; 00269 stringstream err; 00270 err << "getMotorCurrentLimit() - d is zero"; 00271 NasaCommonLogging::Logger::log("gov.nasa.robodyn.GripperKinematics", Priority::ERROR, err.str()); 00272 throw runtime_error(err.str()); 00273 } 00274 else 00275 { 00276 return -1; 00277 } 00278 } 00279 00280 double sliderAngle = atan2(x, L4); 00281 00282 if (boost::math::isnan(sliderAngle)) 00283 { 00284 stringstream err; 00285 err << "getMotorCurrentLimit() - sliderAngle is NaN - atan2(" << x << ", " << L4 << ")"; 00286 NasaCommonLogging::Logger::log("gov.nasa.robodyn.GripperKinematics", Priority::ERROR, err.str()); 00287 throw runtime_error(err.str()); 00288 } 00289 00290 double theta2 = boost::math::constants::pi<double>() - (JAW_ANGLE_ZERO_ADJ + sliderAngle + jawAngle); 00291 double a_2 = pow(d, 2) + pow(L3, 2) - (2 * d * L3 * cos(theta2)); 00292 00293 if (a_2 <= 0) 00294 { 00295 if (not BE_SILENT) 00296 { 00297 BE_SILENT = true; 00298 stringstream err; 00299 err << "getMotorCurrentLimit() - a_2 is non-positive"; 00300 NasaCommonLogging::Logger::log("gov.nasa.robodyn.GripperKinematics", Priority::ERROR, err.str()); 00301 throw runtime_error(err.str()); 00302 } 00303 else 00304 { 00305 return -1; 00306 } 00307 } 00308 00309 double a = pow(a_2, 0.5); 00310 double pre_alpha = (pow(L3, 2) - pow(d, 2) - a_2) / (-2 * d * a); 00311 00312 if (pre_alpha > 1.0) 00313 { 00314 pre_alpha = 1.0; 00315 } 00316 else if (pre_alpha < -1.0) 00317 { 00318 pre_alpha = -1.0; 00319 } 00320 00321 double alpha = acos(pre_alpha); 00322 00323 if (boost::math::isnan(alpha)) 00324 { 00325 stringstream err; 00326 err << "getMotorCurrentLimit() - alpha is NaN - acos(" << pre_alpha << ")"; 00327 NasaCommonLogging::Logger::log("gov.nasa.robodyn.GripperKinematics", Priority::ERROR, err.str()); 00328 throw runtime_error(err.str()); 00329 } 00330 00331 if (L1 == 0) 00332 { 00333 if (not BE_SILENT) 00334 { 00335 BE_SILENT = true; 00336 stringstream err; 00337 err << "getMotorCurrentLimit() - L1 is zero"; 00338 NasaCommonLogging::Logger::log("gov.nasa.robodyn.GripperKinematics", Priority::ERROR, err.str()); 00339 throw runtime_error(err.str()); 00340 } 00341 else 00342 { 00343 return -1; 00344 } 00345 } 00346 00347 double pre_beta = (pow(L2, 2) - pow(L1, 2) - a_2) / (-2 * L1 * a); 00348 00349 if (pre_beta > 1.0) 00350 { 00351 pre_beta = 1.0; 00352 } 00353 else if (pre_beta < -1.0) 00354 { 00355 pre_beta = -1.0; 00356 } 00357 00358 double beta = acos(pre_beta); 00359 00360 if (boost::math::isnan(beta)) 00361 { 00362 if (not BE_SILENT) 00363 { 00364 stringstream err; 00365 err << "getMotorCurrentLimit() - beta is NaN - acos(" << pre_beta << ")"; 00366 NasaCommonLogging::Logger::log("gov.nasa.robodyn.GripperKinematics", Priority::ERROR, err.str()); 00367 throw runtime_error(err.str()); 00368 } 00369 else 00370 { 00371 return -1; 00372 } 00373 } 00374 00375 double theta1 = alpha + beta; 00376 00377 if (L2 == 0) 00378 { 00379 if (not BE_SILENT) 00380 { 00381 BE_SILENT = true; 00382 stringstream err; 00383 err << "getMotorCurrentLimit() - L2 is zero"; 00384 NasaCommonLogging::Logger::log("gov.nasa.robodyn.GripperKinematics", Priority::ERROR, err.str()); 00385 throw runtime_error(err.str()); 00386 } 00387 else 00388 { 00389 return -1; 00390 } 00391 } 00392 00393 double pre_gamma = (pow(d, 2) + pow(L3, 2) - pow(L1, 2) - pow(L2, 2) - (2 * d * L3 * cos(theta2))) / (-2 * L1 * L2); 00394 00395 if (pre_gamma > 1.0) 00396 { 00397 pre_gamma = 1.0; 00398 } 00399 else if (pre_gamma < -1.0) 00400 { 00401 pre_gamma = -1.0; 00402 } 00403 00404 double gamma = acos(pre_gamma); 00405 00406 if (boost::math::isnan(gamma)) 00407 { 00408 if (not BE_SILENT) 00409 { 00410 stringstream err; 00411 err << "getMotorCurrentLimit() - gamma is NaN - acos(" << pre_gamma << ")"; 00412 NasaCommonLogging::Logger::log("gov.nasa.robodyn.GripperKinematics", Priority::ERROR, err.str()); 00413 throw runtime_error(err.str()); 00414 } 00415 else 00416 { 00417 return -1; 00418 } 00419 } 00420 00421 double mu = (2 * boost::math::constants::pi<double>()) - theta2 - theta1 - gamma; 00422 double theta3 = theta1 - sliderAngle; 00423 double gripperTorqueLimit = forceLimit * GRIPPER_LENGTH; 00424 00425 if (sin(mu) == 0) 00426 { 00427 if (not BE_SILENT) 00428 { 00429 BE_SILENT = true; 00430 stringstream err; 00431 err << "getMotorCurrentLimit() - sin(mu) is zero - sin(" << mu << ")"; 00432 NasaCommonLogging::Logger::log("gov.nasa.robodyn.GripperKinematics", Priority::ERROR, err.str()); 00433 throw runtime_error(err.str()); 00434 } 00435 else 00436 { 00437 return -1; 00438 } 00439 } 00440 00441 if (L3 == 0) 00442 { 00443 if (not BE_SILENT) 00444 { 00445 BE_SILENT = true; 00446 stringstream err; 00447 err << "getMotorCurrentLimit() - L3 is zero"; 00448 NasaCommonLogging::Logger::log("gov.nasa.robodyn.GripperKinematics", Priority::ERROR, err.str()); 00449 throw runtime_error(err.str()); 00450 } 00451 else 00452 { 00453 return -1; 00454 } 00455 } 00456 00457 double F2 = gripperTorqueLimit / (L3 * sin(mu)); 00458 00459 if (cos(theta3) == 0) 00460 { 00461 if (not BE_SILENT) 00462 { 00463 BE_SILENT = true; 00464 stringstream err; 00465 err << "getMotorCurrentLimit() - cos(theta3) is zero - cos(" << theta3 << ")"; 00466 NasaCommonLogging::Logger::log("gov.nasa.robodyn.GripperKinematics", Priority::ERROR, err.str()); 00467 throw runtime_error(err.str()); 00468 } 00469 else 00470 { 00471 return -1; 00472 } 00473 } 00474 00475 double FBallscrew = F2 * sin(gamma) / cos(theta3); 00476 00477 if (BALL_SCREW_FORCE_RATIO == 0) 00478 { 00479 if (not BE_SILENT) 00480 { 00481 BE_SILENT = true; 00482 stringstream err; 00483 err << "getMotorCurrentLimit() - BALL_SCREW_FORCE_RATIO is zero"; 00484 NasaCommonLogging::Logger::log("gov.nasa.robodyn.GripperKinematics", Priority::ERROR, err.str()); 00485 throw runtime_error(err.str()); 00486 } 00487 else 00488 { 00489 return -1; 00490 } 00491 } 00492 00493 double motorTorque = FBallscrew / BALL_SCREW_FORCE_RATIO; 00494 00495 if (MOTOR_TORQUE_CONSTANT == 0) 00496 { 00497 if (not BE_SILENT) 00498 { 00499 BE_SILENT = true; 00500 stringstream err; 00501 err << "getMotorCurrentLimit() - MOTOR_TORQUE_CONSTANT is zero"; 00502 NasaCommonLogging::Logger::log("gov.nasa.robodyn.GripperKinematics", Priority::ERROR, err.str()); 00503 throw runtime_error(err.str()); 00504 } 00505 else 00506 { 00507 return -1; 00508 } 00509 } 00510 00511 double motorCurrent = motorTorque / MOTOR_TORQUE_CONSTANT; 00512 double motorCurrentLimit = std::abs(motorCurrent) + COULOMB_FRICTION_CURRENT_OFFSET; 00513 00514 if (boost::math::isnan(motorCurrentLimit)) 00515 { 00516 if (not BE_SILENT) 00517 { 00518 stringstream err; 00519 err << "getMotorCurrentLimit() - motorCurrentLimit is NaN"; 00520 NasaCommonLogging::Logger::log("gov.nasa.robodyn.GripperKinematics", Priority::ERROR, err.str()); 00521 throw runtime_error(err.str()); 00522 } 00523 else 00524 { 00525 return -1; 00526 } 00527 } 00528 00529 return motorCurrentLimit; 00530 }