00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055 #include <ati_force_torque/force_torque_sensor.h>
00056
00057 ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), calibration_params_{nh.getNamespace()+"/Calibration/Offset"}, CS_params_{nh.getNamespace()}, can_params_{nh.getNamespace()+"/CAN"}, FTS_params_{nh.getNamespace()+"/FTS"}, pub_params_{nh.getNamespace()+"/Publish"}, node_params_{nh.getNamespace()+"/Node"}, gravity_params_{nh.getNamespace()+"/GravityCompensation/params"}
00058 {
00059 calibration_params_.fromParamServer();
00060 CS_params_.fromParamServer();
00061 can_params_.fromParamServer();
00062 FTS_params_.fromParamServer();
00063 pub_params_.fromParamServer();
00064 node_params_.fromParamServer();
00065 gravity_params_.fromParamServer();
00066
00067 int calibNMeas;
00068 calibNMeas=calibration_params_.n_measurements;
00069
00070 if (calibNMeas <= 0)
00071 {
00072 ROS_WARN("Parameter 'Calibration/n_measurements' is %d (<=0) using default: 20", calibNMeas);
00073 calibrationNMeasurements = 20;
00074 }
00075 else {
00076 calibrationNMeasurements = (uint)calibNMeas;
00077 }
00078 calibrationTBetween = calibration_params_.T_between_meas;
00079 m_staticCalibration = calibration_params_.isStatic;
00080
00081 std::map<std::string,double> forceVal,torqueVal;
00082 forceVal = calibration_params_.force;
00083 torqueVal = calibration_params_.torque;
00084
00085 m_calibOffset.force.x = forceVal["x"];
00086 m_calibOffset.force.y = forceVal["y"];
00087 m_calibOffset.force.z = forceVal["z"];
00088 m_calibOffset.torque.x = torqueVal["x"];
00089 m_calibOffset.torque.x = torqueVal["y"];
00090 m_calibOffset.torque.x = torqueVal["z"];
00091
00092 bool isAutoInit = false;
00093 m_isInitialized = false;
00094 m_isCalibrated = false;
00095 srvServer_Init_ = nh_.advertiseService("Init", &ForceTorqueSensor::srvCallback_Init, this);
00096 srvServer_CalculateAverageMasurement_ = nh_.advertiseService("CalculateAverageMasurement", &ForceTorqueSensor::srvCallback_CalculateAverageMasurement, this);
00097 srvServer_CalculateOffset_ = nh_.advertiseService("CalculateOffsets", &ForceTorqueSensor::srvCallback_CalculateOffset, this);
00098 srvServer_DetermineCoordianteSystem_ = nh_.advertiseService("DetermineCoordinateSystem", &ForceTorqueSensor::srvCallback_DetermineCoordinateSystem, this);
00099 srvServer_Temp_ = nh_.advertiseService("GetTemperature", &ForceTorqueSensor::srvReadDiagnosticVoltages, this);
00100 srvServer_ReCalibrate = nh_.advertiseService("Recalibrate", &ForceTorqueSensor::srvCallback_recalibrate, this);
00101
00102 reconfigCalibrationSrv_.setCallback(boost::bind(&ForceTorqueSensor::reconfigureCalibrationRequest, this, _1, _2));
00103
00104
00105 canType = can_params_.type;
00106 canPath = can_params_.path;
00107 canBaudrate = can_params_.baudrate;
00108 ftsBaseID = FTS_params_.base_identifier;
00109 isAutoInit = FTS_params_.auto_init;
00110 nodePubFreq = node_params_.ft_pub_freq;
00111 nodePullFreq = node_params_.ft_pull_freq;
00112 sensor_frame_ = node_params_.sensor_frame;
00113
00114 coordinateSystemNMeasurements = CS_params_.n_measurements;
00115 coordinateSystemTBetween = CS_params_.T_between_meas;
00116 coordinateSystemPushDirection = CS_params_.push_direction;
00117
00118 p_tfBuffer = new tf2_ros::Buffer();
00119 p_tfListener = new tf2_ros::TransformListener(*p_tfBuffer, true);
00120
00121
00122 is_pub_gravity_compensated_ = pub_params_.gravity_compensated;
00123 if(is_pub_gravity_compensated_){
00124 gravity_compensated_pub_ = new realtime_tools::RealtimePublisher<geometry_msgs::WrenchStamped>(nh_, "gravity_compensated", 1);
00125 }
00126 is_pub_low_pass_ = pub_params_.low_pass;
00127 if(is_pub_low_pass_){
00128 low_pass_pub_ = new realtime_tools::RealtimePublisher<geometry_msgs::WrenchStamped>(nh_, "low_pass", 1);
00129 }
00130 is_pub_moving_mean_=pub_params_.moving_mean;
00131 if(is_pub_moving_mean_){
00132 moving_mean_pub_ = new realtime_tools::RealtimePublisher<geometry_msgs::WrenchStamped>(nh_, "moving_mean", 1);
00133 }
00134 is_pub_sensor_data_=pub_params_.sensor_data;
00135 if(is_pub_sensor_data_){
00136 sensor_data_pub_ = new realtime_tools::RealtimePublisher<geometry_msgs::WrenchStamped>(nh_, "sensor_data", 1);
00137 }
00138 is_pub_threshold_filtered_ =pub_params_.threshold_filtered;
00139 if(is_pub_threshold_filtered_){
00140 threshold_filtered_pub_ = new realtime_tools::RealtimePublisher<geometry_msgs::WrenchStamped>(nh_, "threshold_filtered", 1);
00141 }
00142 is_pub_transformed_data_ = pub_params_.transformed_data;
00143 if(is_pub_transformed_data_){
00144 transformed_data_pub_ = new realtime_tools::RealtimePublisher<geometry_msgs::WrenchStamped>(nh_, "transformed_data", 1);
00145 }
00146
00147 ftUpdateTimer_ = nh.createTimer(ros::Rate(nodePubFreq), &ForceTorqueSensor::updateFTData, this, false, false);
00148 ftPullTimer_ = nh.createTimer(ros::Rate(nodePullFreq), &ForceTorqueSensor::pullFTData, this, false, false);
00149
00150 moving_mean_filter_->configure("MovingMeanFilter");
00151 low_pass_filter_->configure("LowPassFilter");
00152 gravity_compensator_->configure("GravityCompensation");
00153 threshold_filter_->configure("ThresholdFilter");
00154
00155
00156 if(nh_.hasParam("MovingMeanFilter")) {
00157 useMovingMean = true;
00158 }
00159
00160
00161 if(nh_.hasParam("LowPassFilter")) {
00162 useLowPassFilter = true;
00163 }
00164
00165
00166 if(nh_.hasParam("GravityCompensation")) {
00167 useGravityCompensator = true;
00168 }
00169
00170
00171 if(nh_.hasParam("ThresholdFilter")) {
00172 useThresholdFilter = true;
00173 }
00174
00175 if (canType != -1)
00176 {
00177 p_Ftc = new ForceTorqueCtrl(canType, canPath, canBaudrate, ftsBaseID);
00178 }
00179 else
00180 {
00181 p_Ftc = new ForceTorqueCtrl();
00182 }
00183
00184 if (isAutoInit)
00185 {
00186 std::string msg;
00187 bool success;
00188 init_sensor(msg, success);
00189 ROS_INFO("Autoinit: %s", msg.c_str());
00190 }
00191 }
00192
00193 void ForceTorqueSensor::init_sensor(std::string& msg, bool& success)
00194 {
00195 if (!m_isInitialized)
00196 {
00197
00198 if (p_Ftc->Init())
00199 {
00200
00201 ftPullTimer_.start();
00202
00203
00204
00205 m_isInitialized = true;
00206 success = true;
00207 msg = "FTS initalised!";
00208
00209
00210 if (m_staticCalibration)
00211 {
00212 ROS_INFO("Using static calibration from paramter server with parametes Force: x:%f, y:%f, z:%f; Torque: x: %f, y:%f, z:%f;",
00213 m_calibOffset.force.x, m_calibOffset.force.y, m_calibOffset.force.z,
00214 m_calibOffset.torque.x, m_calibOffset.torque.y, m_calibOffset.torque.z
00215 );
00216 offset_.force.x = m_calibOffset.force.x;
00217 offset_.force.y = m_calibOffset.force.y;
00218 offset_.force.z = m_calibOffset.force.z;
00219 offset_.torque.x= m_calibOffset.torque.x;
00220 offset_.torque.y = m_calibOffset.torque.y;
00221 offset_.torque.z = m_calibOffset.torque.z;
00222 }
00223 else
00224 {
00225 ROS_INFO("Calibrating sensor. Plase wait...");
00226 geometry_msgs::Wrench temp_offset;
00227 if (not calibrate(true, &temp_offset))
00228 {
00229 success = false;
00230 msg = "Calibration failed! :/";
00231 }
00232 }
00233
00234 apply_offset = true;
00235
00236 }
00237 else
00238 {
00239 m_isInitialized = false;
00240 success = false;
00241 msg = "FTS could not be initilised! :/";
00242 }
00243 ftUpdateTimer_.start();
00244 }
00245 }
00246
00247 bool ForceTorqueSensor::srvCallback_Init(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
00248 {
00249 std::string msg;
00250 bool success;
00251
00252 init_sensor(msg, success);
00253 res.message = msg;
00254 res.success = success;
00255
00256 return true;
00257 }
00258
00259 bool ForceTorqueSensor::srvCallback_CalculateAverageMasurement(ati_force_torque::CalculateAverageMasurement::Request& req, ati_force_torque::CalculateAverageMasurement::Response& res)
00260 {
00261 if (m_isInitialized)
00262 {
00263 res.success = true;
00264 res.message = "Measurement successfull! :)";
00265 res.measurement = makeAverageMeasurement(req.N_measurements, req.T_between_meas, req.frame_id);
00266 }
00267 else
00268 {
00269 res.success = false;
00270 res.message = "FTS not initialised! :/";
00271 }
00272
00273 return true;
00274 }
00275
00276 bool ForceTorqueSensor::srvCallback_CalculateOffset(ati_force_torque::CalculateSensorOffset::Request& req, ati_force_torque::CalculateSensorOffset::Response& res)
00277 {
00278 if (m_isInitialized)
00279 {
00280 if (calibrate(req.apply_after_calculation, &res.offset))
00281 {
00282 res.success = true;
00283 res.message = "Calibration successfull! :)";
00284 }
00285 else
00286 {
00287 res.success = false;
00288 res.message = "Calibration failed! :/";
00289 }
00290 }
00291 else
00292 {
00293 res.success = false;
00294 res.message = "FTS not initialised! :/";
00295 }
00296
00297 return true;
00298 }
00299
00300 bool ForceTorqueSensor::srvCallback_recalibrate(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
00301 {
00302 if (!m_isInitialized)
00303 {
00304 ROS_WARN("FTS-Node is not initialized, please initialize first!");
00305 res.success = false;
00306 res.message = "Failed to recalibrate because Node is not initiliazed.";
00307 return true;
00308 }
00309 if (!(nh_.hasParam("force") && nh_.hasParam("CoG_x") && nh_.hasParam("CoG_y") && nh_.hasParam("CoG_z")))
00310 {
00311 ROS_ERROR("Cannot use dynamic recalibration without all values for Gravity Compensation, set parameters or use "
00312 "'Calibrate' service instead.");
00313 res.success = false;
00314 res.message = "Failed to recalibrate because of missing Parameters for Gravity Compensation.";
00315 return true;
00316 }
00317 geometry_msgs::Vector3Stamped gravity, gravity_transformed;
00318 geometry_msgs::Vector3 cog;
00319 double force_value;
00320 cog.x = gravity_params_.CoG_x;
00321 cog.y = gravity_params_.CoG_y;
00322 cog.z = gravity_params_.CoG_z;
00323 force_value = gravity_params_.force;
00324 gravity.vector.z = -force_value;
00325 tf2::doTransform(gravity, gravity_transformed,
00326 p_tfBuffer->lookupTransform(sensor_frame_, transform_frame_, ros::Time(0)));
00327 geometry_msgs::Wrench offset;
00328 calibrate(false, &offset);
00329 offset_.force.x -= gravity_transformed.vector.x;
00330 offset_.force.y -= gravity_transformed.vector.y;
00331 offset_.force.z -= gravity_transformed.vector.z;
00332 offset_.torque.x -= (gravity_transformed.vector.y * cog.z - gravity_transformed.vector.z * cog.y);
00333 offset_.torque.y -= (gravity_transformed.vector.z * cog.x - gravity_transformed.vector.x * cog.z);
00334 offset_.torque.z -= (gravity_transformed.vector.x * cog.y - gravity_transformed.vector.y * cog.x);
00335 res.success = true;
00336 res.message = "Successfully recalibrated FTS!";
00337 return true;
00338 }
00339
00340 bool ForceTorqueSensor::calibrate(bool apply_after_calculation, geometry_msgs::Wrench *new_offset)
00341 {
00342 apply_offset = false;
00343 ROS_INFO("Calibrating using %d measurements and %f s pause between measurements.", calibrationNMeasurements, calibrationTBetween);
00344 geometry_msgs::Wrench temp_offset = makeAverageMeasurement(calibrationNMeasurements, calibrationTBetween);
00345
00346 apply_offset = true;
00347 if (apply_after_calculation) {
00348 offset_ = temp_offset;
00349 }
00350
00351 ROS_INFO("Calibration Data: Fx: %f; Fy: %f; Fz: %f; Mx: %f; My: %f; Mz: %f", temp_offset.force.x, temp_offset.force.y, temp_offset.force.z, temp_offset.torque.x, temp_offset.torque.y, temp_offset.torque.z);
00352
00353 m_isCalibrated = true;
00354 *new_offset = temp_offset;
00355
00356 return m_isCalibrated;
00357 }
00358
00359 geometry_msgs::Wrench ForceTorqueSensor::makeAverageMeasurement(uint number_of_measurements, double time_between_meas, std::string frame_id)
00360 {
00361 geometry_msgs::Wrench measurement;
00362 int num_of_errors = 0;
00363 ros::Duration duration(time_between_meas);
00364 for (int i = 0; i < number_of_measurements; i++)
00365 {
00366 geometry_msgs::Wrench output;
00367
00368 if (frame_id.compare("") != 0) {
00369 if (not transform_wrench(frame_id, sensor_frame_, moving_mean_filtered_wrench.wrench, &output))
00370 {
00371 num_of_errors++;
00372 if (num_of_errors > 200){
00373 return measurement;
00374 }
00375 i--;
00376 continue;
00377 }
00378 }
00379 else
00380 {
00381 output = moving_mean_filtered_wrench.wrench;
00382 }
00383 measurement.force.x += output.force.x;
00384 measurement.force.y += output.force.y;
00385 measurement.force.z += output.force.z;
00386 measurement.torque.x += output.torque.x;
00387 measurement.torque.y += output.torque.y;
00388 measurement.torque.z+= output.torque.z;
00389 duration.sleep();
00390 }
00391 measurement.force.x /= number_of_measurements;
00392 measurement.force.y /= number_of_measurements;
00393 measurement.force.z /= number_of_measurements;
00394 measurement.torque.x /= number_of_measurements;
00395 measurement.torque.y /= number_of_measurements;
00396 measurement.torque.z /= number_of_measurements;
00397 return measurement;
00398 }
00399
00400
00401
00402 bool ForceTorqueSensor::srvCallback_DetermineCoordinateSystem(std_srvs::Trigger::Request& req,
00403 std_srvs::Trigger::Response& res)
00404 {
00405 if (m_isInitialized && m_isCalibrated)
00406 {
00407 double angle;
00408
00409 ROS_INFO("Please push FTS with force larger than 10 N in desired direction of new axis %d",
00410 coordinateSystemPushDirection);
00411
00412 for (int i = 0; i < coordinateSystemNMeasurements; i++)
00413 {
00414 int status = 0;
00415 double Fx, Fy, Fz, Tx, Ty, Tz = 0;
00416 p_Ftc->ReadSGData(status, Fx, Fy, Fz, Tx, Ty, Tz);
00417
00418 angle += atan2(Fy, Fx);
00419
00420 usleep(coordinateSystemTBetween);
00421 }
00422
00423 angle /= coordinateSystemNMeasurements;
00424
00425 if (coordinateSystemPushDirection)
00426 {
00427 angle -= M_PI / 2;
00428 }
00429
00430 ROS_INFO("Please rotate your coordinate system for %f rad (%f deg) around z-axis", angle, angle / M_PI * 180.0);
00431
00432 res.success = true;
00433 res.message = "CoordianteSystem successfull! :)";
00434 }
00435 else
00436 {
00437 res.success = false;
00438 res.message = "FTS not initialised or not calibrated! :/";
00439 }
00440
00441 return true;
00442 }
00443
00444 bool ForceTorqueSensor::srvReadDiagnosticVoltages(ati_force_torque::DiagnosticVoltages::Request& req,
00445 ati_force_torque::DiagnosticVoltages::Response& res)
00446 {
00447 p_Ftc->ReadDiagnosticADCVoltages(req.index, res.adc_value);
00448
00449 return true;
00450 }
00451
00452 void ForceTorqueSensor::pullFTData(const ros::TimerEvent &event)
00453 {
00454 int status = 0;
00455
00456 bool bRet = p_Ftc->ReadSGData(status, sensor_data.wrench.force.x, sensor_data.wrench.force.y, sensor_data.wrench.force.z,
00457 sensor_data.wrench.torque.x, sensor_data.wrench.torque.y, sensor_data.wrench.torque.z);
00458 if (bRet != false)
00459 {
00460 sensor_data.header.stamp = ros::Time::now();
00461 sensor_data.header.frame_id = sensor_frame_;
00462 if (apply_offset) {
00463 sensor_data.wrench.force.x -= offset_.force.x;
00464 sensor_data.wrench.force.y -= offset_.force.y;
00465 sensor_data.wrench.force.z -= offset_.force.z;
00466 sensor_data.wrench.torque.x -= offset_.torque.x;
00467 sensor_data.wrench.torque.y -= offset_.torque.y;
00468 sensor_data.wrench.torque.z -= offset_.torque.z;
00469 }
00470
00471
00472 low_pass_filtered_data.header = sensor_data.header;
00473 if(useLowPassFilter){
00474 std::vector<double> in_data= {(double)sensor_data.wrench.force.x, double(sensor_data.wrench.force.y), (double)sensor_data.wrench.force.z,(double)sensor_data.wrench.torque.x,(double)sensor_data.wrench.torque.y,(double)sensor_data.wrench.torque.z};
00475 std::vector<double> out_data= {(double)low_pass_filtered_data.wrench.force.x, double(low_pass_filtered_data.wrench.force.y), (double)low_pass_filtered_data.wrench.force.z,(double)low_pass_filtered_data.wrench.torque.x,(double)low_pass_filtered_data.wrench.torque.y,(double)low_pass_filtered_data.wrench.torque.z};
00476
00477 low_pass_filter_->update(sensor_data,low_pass_filtered_data);
00478 }
00479 else low_pass_filtered_data = sensor_data;
00480
00481 moving_mean_filtered_wrench.header = low_pass_filtered_data.header;
00482 if(useMovingMean){
00483 std::vector<double> in_data= {(double)low_pass_filtered_data.wrench.force.x, double(low_pass_filtered_data.wrench.force.y), (double)low_pass_filtered_data.wrench.force.z,(double)low_pass_filtered_data.wrench.torque.x,(double)low_pass_filtered_data.wrench.torque.y,(double)low_pass_filtered_data.wrench.torque.z};
00484 std::vector<double> out_data = {(double)moving_mean_filtered_wrench.wrench.force.x, double(moving_mean_filtered_wrench.wrench.force.y), (double)moving_mean_filtered_wrench.wrench.force.z,(double)moving_mean_filtered_wrench.wrench.torque.x,(double)moving_mean_filtered_wrench.wrench.torque.y,(double)moving_mean_filtered_wrench.wrench.torque.z};
00485
00486 moving_mean_filter_->update(low_pass_filtered_data, moving_mean_filtered_wrench);
00487 }
00488 else moving_mean_filtered_wrench = low_pass_filtered_data;
00489
00490 if(is_pub_sensor_data_)
00491 if (sensor_data_pub_->trylock()){
00492 sensor_data_pub_->msg_ = sensor_data;
00493 sensor_data_pub_->unlockAndPublish();
00494 }
00495
00496 if(is_pub_low_pass_)
00497 if (low_pass_pub_->trylock()){
00498 low_pass_pub_->msg_ = low_pass_filtered_data;
00499 low_pass_pub_->unlockAndPublish();
00500 }
00501
00502 if(is_pub_moving_mean_)
00503 if (moving_mean_pub_->trylock()){
00504 moving_mean_pub_->msg_ = moving_mean_filtered_wrench;
00505 moving_mean_pub_->unlockAndPublish();
00506 }
00507 }
00508 }
00509
00510 void ForceTorqueSensor::filterFTData(){
00511
00512 transformed_data.header.stamp = moving_mean_filtered_wrench.header.stamp;
00513 transformed_data.header.frame_id = transform_frame_;
00514 if (transform_wrench(transform_frame_, sensor_frame_, moving_mean_filtered_wrench.wrench, &transformed_data.wrench))
00515 {
00516
00517 if(useGravityCompensator)
00518 {
00519 std::vector<double> in_data= {(double)transformed_data.wrench.force.x, double(transformed_data.wrench.force.y), (double)transformed_data.wrench.force.z,(double)transformed_data.wrench.torque.x,(double)transformed_data.wrench.torque.y,(double)transformed_data.wrench.torque.z};
00520 std::vector<double> out_data = {(double)gravity_compensated_force.wrench.force.x, double(gravity_compensated_force.wrench.force.y), (double)gravity_compensated_force.wrench.force.z,(double)gravity_compensated_force.wrench.torque.x,(double)gravity_compensated_force.wrench.torque.y,(double)gravity_compensated_force.wrench.torque.z};
00521 gravity_compensator_->update(transformed_data, gravity_compensated_force);
00522 }
00523 else gravity_compensated_force = transformed_data;
00524
00525
00526 if(useThresholdFilter)
00527 {
00528 std::vector<double> in_data= {(double)gravity_compensated_force.wrench.force.x, double(gravity_compensated_force.wrench.force.y), (double)gravity_compensated_force.wrench.force.z,(double)gravity_compensated_force.wrench.torque.x,(double)gravity_compensated_force.wrench.torque.y,(double)gravity_compensated_force.wrench.torque.z};
00529 std::vector<double> out_data = {(double)threshold_filtered_force.wrench.force.x, double(threshold_filtered_force.wrench.force.y), (double)threshold_filtered_force.wrench.force.z,(double)threshold_filtered_force.wrench.torque.x,(double)threshold_filtered_force.wrench.torque.y,(double)threshold_filtered_force.wrench.torque.z};
00530 threshold_filter_->update(gravity_compensated_force, threshold_filtered_force);
00531 }
00532 else threshold_filtered_force = gravity_compensated_force;
00533
00534 if(is_pub_transformed_data_)
00535 if (transformed_data_pub_->trylock()){
00536 transformed_data_pub_->msg_ = transformed_data;
00537 transformed_data_pub_->unlockAndPublish();
00538 }
00539 if(is_pub_gravity_compensated_ && useGravityCompensator)
00540 if (gravity_compensated_pub_->trylock()){
00541 gravity_compensated_pub_->msg_ = gravity_compensated_force;
00542 gravity_compensated_pub_->unlockAndPublish();
00543 }
00544
00545 if(is_pub_threshold_filtered_ && useThresholdFilter)
00546 if (threshold_filtered_pub_->trylock()){
00547 threshold_filtered_pub_->msg_ = threshold_filtered_force;
00548 threshold_filtered_pub_->unlockAndPublish();
00549 }
00550 }
00551 else threshold_filtered_force = moving_mean_filtered_wrench;
00552 }
00553
00554 bool ForceTorqueSensor::transform_wrench(std::string goal_frame, std::string source_frame, geometry_msgs::Wrench wrench, geometry_msgs::Wrench *transformed)
00555 {
00556 geometry_msgs::TransformStamped transform;
00557 geometry_msgs::Vector3Stamped temp_vector_in, temp_vector_out;
00558
00559 try
00560 {
00561 transform = p_tfBuffer->lookupTransform(goal_frame, source_frame, ros::Time(0));
00562 _num_transform_errors = 0;
00563 }
00564 catch (tf2::TransformException ex)
00565 {
00566 if (_num_transform_errors%100 == 0){
00567 ROS_ERROR("%s", ex.what());
00568 }
00569 _num_transform_errors++;
00570 return false;
00571 }
00572
00573 temp_vector_in.vector = wrench.force;
00574 tf2::doTransform(temp_vector_in, temp_vector_out, transform);
00575 transformed->force = temp_vector_out.vector;
00576
00577 temp_vector_in.vector = wrench.torque;
00578 tf2::doTransform(temp_vector_in, temp_vector_out, transform);
00579 transformed->torque = temp_vector_out.vector;
00580
00581 return true;
00582 }
00583
00584 void ForceTorqueSensor::reconfigureCalibrationRequest(ati_force_torque::CalibrationConfig& config, uint32_t level){
00585 calibration_params_.fromConfig(config);
00586
00587 calibrationTBetween = calibration_params_.T_between_meas;
00588 m_staticCalibration = calibration_params_.isStatic;
00589
00590 std::map<std::string,double> forceVal,torqueVal;
00591 forceVal = calibration_params_.force;
00592 torqueVal = calibration_params_.torque;
00593
00594 m_calibOffset.force.x = forceVal["x"];
00595 m_calibOffset.force.y = forceVal["y"];
00596 m_calibOffset.force.z = forceVal["z"];
00597 m_calibOffset.torque.x = torqueVal["x"];
00598 m_calibOffset.torque.x = torqueVal["y"];
00599 m_calibOffset.torque.x = torqueVal["z"];
00600
00601 }