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 #include <pr2_gripper_sensor_controller/pr2_gripper_sensor_controller.h>
00049 #include <pluginlib/class_list_macros.h>
00050 #include <pr2_hardware_interface/hardware_interface.h>
00051 #include <std_srvs/Empty.h>
00052
00053
00054 using namespace pr2_gripper_sensor_controller;
00055
00056
00069 bool PR2GripperSensorController::initializeHandles(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
00070 {
00071
00072
00073 pr2_hardware_interface::HardwareInterface* hardwareInterface = robot->model_->hw_;
00074 if(!hardwareInterface)
00075 {
00076 ROS_ERROR("Perhaps Something wrong with the hardware interface pointer!!!!");
00077 }
00078
00079
00080 std::string accelerometer_name;
00081 if(!n.getParam("accelerometer_name", accelerometer_name))
00082 {
00083 ROS_ERROR("No accelerometer given in namespace: '%s')", n.getNamespace().c_str());
00084 return false;
00085 }
00086 pr2_hardware_interface::Accelerometer* accelerometerHandle = hardwareInterface->getAccelerometer(accelerometer_name);
00087
00088 if(!accelerometerHandle)
00089 {
00090 ROS_ERROR("PR2GripperSensorController could not find sensor named '%s'", accelerometer_name.c_str());
00091 return false;
00092 }
00093 myAccelerationObserver = new accelerationObserver(accelerometerHandle);
00094
00095
00096 std::string leftFinger_pressureSensor_name;
00097 if(!n.getParam("left_pressure_sensor_name", leftFinger_pressureSensor_name))
00098 {
00099 ROS_ERROR("No accelerometer given in namespace: '%s')", n.getNamespace().c_str());
00100 return false;
00101 }
00102 pr2_hardware_interface::PressureSensor* leftFinger_pressureSensorHandle = hardwareInterface->getPressureSensor(leftFinger_pressureSensor_name);
00103 if(!leftFinger_pressureSensorHandle)
00104 {
00105 ROS_ERROR("PR2GripperSensorController could not find sensor named '%s'", leftFinger_pressureSensor_name.c_str());
00106 return false;
00107 }
00108
00109
00110 std::string rightFinger_pressureSensor_name;
00111 if(!n.getParam("right_pressure_sensor_name", rightFinger_pressureSensor_name))
00112 {
00113 ROS_ERROR("No accelerometer given in namespace: '%s')", n.getNamespace().c_str());
00114 return false;
00115 }
00116 pr2_hardware_interface::PressureSensor* rightFinger_pressureSensorHandle = hardwareInterface->getPressureSensor(rightFinger_pressureSensor_name);
00117 if(!rightFinger_pressureSensorHandle)
00118 {
00119 ROS_ERROR("PR2GripperSensorController could not find sensor named '%s'", rightFinger_pressureSensor_name.c_str());
00120 return false;
00121 }
00122 myPressureObserver = new pressureObserver(leftFinger_pressureSensorHandle, rightFinger_pressureSensorHandle);
00123
00124
00125 std::string joint_name;
00126 if (!n.getParam("joint_name", joint_name))
00127 {
00128 ROS_ERROR("No joint given in namespace: '%s')", n.getNamespace().c_str());
00129 return false;
00130 }
00131 pr2_mechanism_model::JointState* joint_state_ = robot->getJointState(joint_name);
00132 if (!joint_state_)
00133 {
00134 ROS_ERROR("PR2GripperSensorController could not find joint named '%s'", joint_name.c_str());
00135 return false;
00136 }
00137
00138 n.param("publish_skip", publish_skip, 1);
00139
00140
00141 myGripperController = new gripperController(joint_state_, myPressureObserver, myAccelerationObserver);
00142
00143 return true;
00144 }
00145
00146
00158 bool PR2GripperSensorController::reloadParams(std_srvs::Empty::Request& req,std_srvs::Empty::Response& resp)
00159 {
00160 if(!nodeHandle.getParam("close_speed", servo_velocity))
00161 ROS_ERROR("No close_speed given in namespace: '%s')", nodeHandle.getNamespace().c_str());
00162 servo_velocity = -servo_velocity;
00163 if(servo_velocity > 0)
00164 {
00165 ROS_ERROR("Incorrect sign on close_speed (negative speed is impossible), setting to 0!");
00166 servo_velocity = 0;
00167 }
00168
00169 if(!nodeHandle.getParam("max_joint_effort", max_effort))
00170 ROS_ERROR("No max_joint_effort given in namespace: '%s')", nodeHandle.getNamespace().c_str());
00171 myGripperController->max_effort = max_effort;
00172
00173 if(!nodeHandle.getParam("fingertip_force_limit", fingertip_force_limit))
00174 ROS_ERROR("No fingertip_force_limit given in namespace: '%s')", nodeHandle.getNamespace().c_str());
00175 fingertip_force_limit = -fingertip_force_limit;
00176 myGripperController->fingertip_force_limit = fingertip_force_limit;
00177
00178 if(!nodeHandle.getParam("deformation_limit", deformation_limit))
00179 ROS_ERROR("No deformation_limit given in namespace: '%s')", nodeHandle.getNamespace().c_str());
00180 myGripperController->positionMarker_limit = deformation_limit;
00181
00182 if(!nodeHandle.getParam("slip_servo_start_force", fingertip_start_force))
00183 ROS_ERROR("No slip_servo_start_force given in namespace: '%s')", nodeHandle.getNamespace().c_str());
00184 fingertip_start_force = -fingertip_start_force;
00185
00186 if(!nodeHandle.getParam("force_lightest", myPressureObserver->forceLightest))
00187 ROS_ERROR("No force_lightest given in namespace: '%s')", nodeHandle.getNamespace().c_str());
00188
00189 if(!nodeHandle.getParam("hp_force_trigger", myPressureObserver->hpForceTrigger))
00190 ROS_ERROR("No hpForceTrigger given in namespace: '%s')", nodeHandle.getNamespace().c_str());
00191
00192 if(!nodeHandle.getParam("force_servo_force_tolerance", myGripperController->force_servo_force_tolerance))
00193 ROS_ERROR("No force_servo_force_tolerance given in namespace: '%s')", nodeHandle.getNamespace().c_str());
00194
00195 if(!nodeHandle.getParam("force_servo_velocity_tolerance", force_servo_velocity_tolerance))
00196 ROS_ERROR("No force_servo_velocity_tolerance given in namespace: '%s')", nodeHandle.getNamespace().c_str());
00197
00198 if(!nodeHandle.getParam("position_servo_position_tolerance", force_servo_velocity_tolerance))
00199 ROS_ERROR("No position_servo_position_tolerance given in namespace: '%s')", nodeHandle.getNamespace().c_str());
00200
00201
00202 return true;
00203
00204 }
00205
00206
00207
00221 bool PR2GripperSensorController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
00222 {
00223
00224 publish_raw_data = false;
00225
00226
00227 assert(robot);
00228 nodeHandle = n;
00229 robotHandle = robot;
00230
00231
00232 bool success = this->initializeHandles(robot, n);
00233 loop_count_ = 0;
00234
00235
00236 control_mode = rt_state_def.DISABLED;
00237
00238
00239 last_time_ = robot->getTime();
00240
00241
00242 updateZeros_srv_ = n.advertiseService("zero_fingertip_sensors", &PR2GripperSensorController::updateZeros, this);
00243 reloadParams_srv_ = n.advertiseService("reload_params", &PR2GripperSensorController::reloadParams, this);
00244 stopMotorOutput_srv_ = n.advertiseService("stop_motor_output", &PR2GripperSensorController::stopMotorOutput, this);
00245
00246
00247 if(publish_raw_data)
00248 {
00249 raw_data_publisher_.reset(new realtime_tools::RealtimePublisher<pr2_gripper_sensor_msgs::PR2GripperSensorRawData>(n, "raw_data", 1));
00250 }
00251
00252
00253 controller_state_publisher_.reset(new realtime_tools::RealtimePublisher<pr2_controllers_msgs::JointControllerState>(n, "state", 1));
00254
00255
00256 contact_state_publisher_.reset(new realtime_tools::RealtimePublisher<pr2_gripper_sensor_msgs::PR2GripperFindContactData>(n, "contact_state", 1));
00257
00258
00259 event_detector_state_publisher_.reset(new realtime_tools::RealtimePublisher<pr2_gripper_sensor_msgs::PR2GripperEventDetectorData>(n, "event_detector_state", 1));
00260
00261
00262 slip_state_publisher_.reset(new realtime_tools::RealtimePublisher<pr2_gripper_sensor_msgs::PR2GripperSlipServoData>(n, "slip_servo_state", 1));
00263
00264
00265 force_state_publisher_.reset(new realtime_tools::RealtimePublisher<pr2_gripper_sensor_msgs::PR2GripperForceServoData>(n, "force_servo_state", 1));
00266
00267
00268 sub_command_ = n.subscribe<pr2_controllers_msgs::Pr2GripperCommand>("command", 1, &PR2GripperSensorController::positionCB, this);
00269
00270
00271 sub_findcontact_command_ = n.subscribe<pr2_gripper_sensor_msgs::PR2GripperFindContactCommand>("find_contact", 1, &PR2GripperSensorController::findContactCB, this);
00272
00273
00274 sub_event_detector_command_ = n.subscribe<pr2_gripper_sensor_msgs::PR2GripperEventDetectorCommand>("event_detector", 1, &PR2GripperSensorController::eventDetectorCB, this);
00275
00276
00277 sub_slipservo_command_ = n.subscribe<pr2_gripper_sensor_msgs::PR2GripperSlipServoCommand>("slip_servo", 1, &PR2GripperSensorController::slipServoCB, this);
00278
00279
00280 sub_forceservo_command_ = n.subscribe<pr2_gripper_sensor_msgs::PR2GripperForceServoCommand>("force_servo", 1, &PR2GripperSensorController::forceServoCB, this);
00281
00282
00283 std_srvs::Empty::Request req;
00284 std_srvs::Empty::Response rsp;
00285 updateZeros(req,rsp);
00286
00287
00288 reloadParams(req,rsp);
00289
00290 return success;
00291
00292 }
00293
00294
00295
00304 void PR2GripperSensorController::update()
00305 {
00306
00307
00308 ros::Time time = robotHandle->getTime();
00309 ros::Duration dt = time - last_time_;
00310
00311
00312 assert(robotHandle != NULL);
00313
00314
00315
00316
00317
00318 if(control_mode == rt_state_def.DISABLED){}
00319
00320
00321 else if(control_mode == rt_state_def.POSITION_SERVO)
00322 {
00323
00324
00325 myGripperController->positionServo(servo_position,0.0);
00326
00327 myGripperController->positionMarker = myGripperController->positionCurrent;
00328 }
00329
00330
00331 else if(control_mode == rt_state_def.FORCE_SERVO)
00332 {
00333
00334
00335 if( ( (myGripperController->forceServo2(servo_force)) &&
00336 fabs(myGripperController->lpVelocity) < force_servo_velocity_tolerance))
00337
00338 {
00339
00340 if(findContact_delay > 250)
00341 stable_force = true;
00342 findContact_delay++;
00343 }
00344 }
00345
00346
00347 else if(control_mode == rt_state_def.FIND_CONTACT)
00348 {
00349
00350 if(!myGripperController->grabObject(servo_velocity, contacts_desired))
00351 {
00352 contact_success = false;
00353
00354 myGripperController->vel_integral = myGripperController->gripper_state_now_position;
00355
00356 myGripperController->positionMarker = myGripperController->positionCurrent;
00357 }
00358
00359 else
00360 {
00361
00362
00363 if(contactCounter < 50)
00364 {
00365 servo_position = myGripperController->positionContact;
00366
00367
00368 servo_force = myGripperController->forceContact;
00369
00370
00371 myGripperController->positionServo(servo_position,0.0);
00372 }
00373
00374 else
00375 {
00376 myGripperController->positionServo(servo_position,0.0);
00377 stable_contact = true;
00378 control_mode = rt_state_def.POSITION_SERVO;
00379 }
00380
00381 contactCounter++;
00382 contact_success = true;
00383 }
00384 }
00385
00386
00387 else if(control_mode == rt_state_def.SLIP_SERVO)
00388 myGripperController->slipServo2();
00389
00390
00391
00392 else
00393 {
00394 ROS_ERROR("PR2_GRIPPER_SENSOR_CONTROLLER ENTERED A BAD STATE MACHINE STATE");
00395 myGripperController->jointState->commanded_effort_ = 0;
00396
00397 control_mode = rt_state_def.DISABLED;
00398 }
00399
00400
00401
00402
00403
00404
00405
00406
00407 if(myGripperController->place(placeConditions,acc_trigger,slip_trigger))
00408 placedState = true;
00409
00410
00411 if(update_zeros == true)
00412 myPressureObserver->updateZeros2();
00413
00414
00415
00416
00417
00418 if(loop_count_ % publish_skip == 0)
00419 {
00420 if(controller_state_publisher_ && controller_state_publisher_->trylock())
00421 {
00422 controller_state_publisher_->msg_.header.stamp = time;
00423 controller_state_publisher_->msg_.set_point = servo_position;
00424 controller_state_publisher_->msg_.process_value = myGripperController->gripper_state_now_position;
00425 controller_state_publisher_->msg_.process_value_dot = myGripperController->gripper_state_now_velocity;
00426 controller_state_publisher_->msg_.error = myGripperController->gripper_state_now_position - servo_position;
00427 controller_state_publisher_->msg_.time_step = dt.toSec();
00428 controller_state_publisher_->msg_.command = myGripperController->gripper_state_now_measured_effort;
00429
00430 controller_state_publisher_->unlockAndPublish();
00431 }
00432
00433 if(contact_state_publisher_ && contact_state_publisher_->trylock())
00434 {
00435 contact_state_publisher_->msg_.stamp = time;
00436 contact_state_publisher_->msg_.left_fingertip_pad_contact = myPressureObserver->left_contact;
00437 contact_state_publisher_->msg_.right_fingertip_pad_contact = myPressureObserver->right_contact;
00438 contact_state_publisher_->msg_.left_fingertip_pad_force = myPressureObserver->padForce_left_cur_nonbiased;
00439 contact_state_publisher_->msg_.right_fingertip_pad_force = myPressureObserver->padForce_right_cur_nonbiased;
00440 contact_state_publisher_->msg_.joint_position = myGripperController->positionContact;
00441 contact_state_publisher_->msg_.joint_effort = myGripperController->gripper_state_now_measured_effort;
00442 contact_state_publisher_->msg_.rtstate.realtime_controller_state = control_mode;
00443 contact_state_publisher_->msg_.contact_conditions_met = stable_contact;
00444
00445
00446 contact_state_publisher_->unlockAndPublish();
00447 }
00448
00449 if(slip_state_publisher_ && slip_state_publisher_->trylock())
00450 {
00451 slip_state_publisher_->msg_.stamp = time;
00452 slip_state_publisher_->msg_.joint_effort = myGripperController->gripper_state_now_measured_effort;
00453 slip_state_publisher_->msg_.left_fingertip_pad_force = myPressureObserver->padForce_left_cur_nonbiased;
00454 slip_state_publisher_->msg_.right_fingertip_pad_force = myPressureObserver->padForce_right_cur_nonbiased;
00455 slip_state_publisher_->msg_.deformation = myGripperController->positionCurrent - myGripperController->positionMarker;
00456 slip_state_publisher_->msg_.slip_detected = myGripperController->slip_flag;
00457 slip_state_publisher_->msg_.fingertip_force_limit_reached = myGripperController->force_limit_flag;
00458 slip_state_publisher_->msg_.deformation_limit_reached = myGripperController->deformation_limit_flag;
00459 slip_state_publisher_->msg_.gripper_empty = myGripperController->dropped_flag;
00460 slip_state_publisher_->msg_.rtstate.realtime_controller_state = control_mode;
00461
00462 slip_state_publisher_->unlockAndPublish();
00463 }
00464
00465 if(force_state_publisher_ && force_state_publisher_->trylock())
00466 {
00467 force_state_publisher_->msg_.stamp = time;
00468 force_state_publisher_->msg_.joint_effort = myGripperController->gripper_state_now_measured_effort;
00469 force_state_publisher_->msg_.left_fingertip_pad_force = myPressureObserver->padForce_left_cur_nonbiased;
00470 force_state_publisher_->msg_.right_fingertip_pad_force = myPressureObserver->padForce_right_cur_nonbiased;
00471 force_state_publisher_->msg_.force_achieved = stable_force;
00472 force_state_publisher_->msg_.rtstate.realtime_controller_state = control_mode;
00473
00474 force_state_publisher_->unlockAndPublish();
00475 }
00476
00477 if(event_detector_state_publisher_ && event_detector_state_publisher_->trylock())
00478 {
00479 event_detector_state_publisher_->msg_.stamp = time;
00480 event_detector_state_publisher_->msg_.trigger_conditions_met = placedState;
00481 event_detector_state_publisher_->msg_.slip_event = myPressureObserver->placeContact;
00482 event_detector_state_publisher_->msg_.acceleration_event = myAccelerationObserver->placeContact;
00483 event_detector_state_publisher_->msg_.acceleration_vector[0] = myAccelerationObserver->aX_bp;
00484 event_detector_state_publisher_->msg_.acceleration_vector[1] = myAccelerationObserver->aY_bp;
00485 event_detector_state_publisher_->msg_.acceleration_vector[2] = myAccelerationObserver->aZ_bp;
00486
00487 event_detector_state_publisher_->unlockAndPublish();
00488 }
00489 }
00490
00491
00492 if(loop_count_ % publish_skip == 0)
00493 {
00494 if(publish_raw_data)
00495 {
00496 raw_data_publisher_->msg_.stamp = time;
00497 raw_data_publisher_->msg_.left_finger_pad_force = myPressureObserver->padForce_left_cur_nonbiased;
00498 raw_data_publisher_->msg_.right_finger_pad_force = myPressureObserver->padForce_right_cur_nonbiased;
00499 raw_data_publisher_->msg_.left_finger_pad_force_filtered = myPressureObserver->padForce_left_cur;
00500 raw_data_publisher_->msg_.right_finger_pad_force_filtered = myPressureObserver->padForce_right_cur;
00501 raw_data_publisher_->msg_.left_finger_pad_forces = myPressureObserver->pressure_current_zerod.pressure_left;
00502 raw_data_publisher_->msg_.right_finger_pad_forces = myPressureObserver->pressure_current_zerod.pressure_right;
00503 raw_data_publisher_->msg_.left_finger_pad_forces_filtered = myPressureObserver->pressure_cur_bias.pressure_left;
00504 raw_data_publisher_->msg_.right_finger_pad_forces_filtered = myPressureObserver->pressure_cur_bias.pressure_right;
00505 raw_data_publisher_->msg_.acc_x_raw = myAccelerationObserver->aX_lp;
00506 raw_data_publisher_->msg_.acc_y_raw = myAccelerationObserver->aY_lp;
00507 raw_data_publisher_->msg_.acc_z_raw = myAccelerationObserver->aZ_lp;
00508 raw_data_publisher_->msg_.acc_x_filtered = myAccelerationObserver->aX_bp;
00509 raw_data_publisher_->msg_.acc_y_filtered = myAccelerationObserver->aY_bp;
00510 raw_data_publisher_->msg_.acc_z_filtered = myAccelerationObserver->aZ_bp;
00511 raw_data_publisher_->msg_.left_contact = myPressureObserver->left_contact;
00512 raw_data_publisher_->msg_.right_contact = myPressureObserver->right_contact;
00513
00514 raw_data_publisher_->unlockAndPublish();
00515 }
00516 }
00517
00518
00519
00520
00521
00522 myGripperController->updateData();
00523
00524
00525 loop_count_++;
00526 last_time_ = time;
00527 }
00528
00535 void PR2GripperSensorController::stopping()
00536 {
00537
00538 myGripperController->jointState->commanded_effort_ = 0;
00539 }
00540
00541
00542
00556 bool PR2GripperSensorController::updateZeros(std_srvs::Empty::Request& req,std_srvs::Empty::Response& resp)
00557 {
00558 ROS_INFO("Updating zeros....");
00559 update_zeros = true;
00560 ros::Duration(0.25).sleep();
00561 update_zeros = false;
00562 ROS_INFO(".... zeros finished updating");
00563
00564 return true;
00565 }
00566
00567
00580 bool PR2GripperSensorController::stopMotorOutput(std_srvs::Empty::Request& req,std_srvs::Empty::Response& resp)
00581 {
00582 ROS_INFO("Stopping gripper command output.");
00583 control_mode = rt_state_def.DISABLED;
00584
00585 return true;
00586 }
00587
00588
00593 void PR2GripperSensorController::reinitializeValues()
00594 {
00595 findContact_delay = 0;
00596 stable_force = false;
00597
00598
00599 myGripperController->firstRamp = true;
00600
00601 myGripperController->vel_integral_vcontrol = myGripperController->positionCurrent;
00602
00603
00604 stable_contact = false;
00605
00606
00607 myGripperController->forceContact = 0;
00608 myGripperController->forceContact_l = 0;
00609 myGripperController->forceContact_r = 0;
00610
00611
00612 myGripperController->positionMarker = myGripperController->positionCurrent;
00613 myGripperController->slip_flag = false;
00614 myGripperController->force_limit_flag = false;
00615 myGripperController->deformation_limit_flag = false;
00616
00617 myGripperController->dropped_flag = false;
00618
00619 }
00620
00621
00630 void PR2GripperSensorController::positionCB(const pr2_controllers_msgs::Pr2GripperCommandConstPtr& msg)
00631 {
00632 reinitializeValues();
00633
00634
00635 servo_position = msg->position;
00636 max_effort = msg->max_effort;
00637 myGripperController->max_effort = max_effort;
00638
00639
00640
00641 nodeHandle.setParam("max_joint_effort", max_effort);
00642
00643
00644
00645 control_mode = rt_state_def.POSITION_SERVO;
00646
00647 ROS_INFO("Gripper Position Servo to: %f", servo_position);
00648 }
00649
00650
00659 void PR2GripperSensorController::findContactCB(const pr2_gripper_sensor_msgs::PR2GripperFindContactCommandConstPtr& msg)
00660 {
00661 if(myPressureObserver->broken_sensor)
00662 {
00663 ROS_ERROR("REFUSING TO FIND CONTACT - PRESSURE SENSOR HAS ZERO READING AND MAY BE MALFUNCTIONING!");
00664 return;
00665 }
00666
00667 reinitializeValues();
00668 contactCounter = 0;
00669
00670
00671 contacts_desired = msg->contact_conditions;
00672
00673
00674 myPressureObserver->left_contact = false;
00675 myPressureObserver->right_contact = false;
00676
00677
00678 control_mode = rt_state_def.FIND_CONTACT;
00679
00680 }
00681
00682
00691 void PR2GripperSensorController::eventDetectorCB(const pr2_gripper_sensor_msgs::PR2GripperEventDetectorCommandConstPtr& msg)
00692 {
00693 placeConditions = msg->trigger_conditions;
00694 acc_trigger = msg->acceleration_trigger_magnitude;
00695 slip_trigger = msg->slip_trigger_magnitude;
00696
00697 placedState = false;
00698 myPressureObserver->placeContact = false;
00699 myAccelerationObserver->placeContact = false;
00700 }
00701
00709 void PR2GripperSensorController::slipServoCB(const pr2_gripper_sensor_msgs::PR2GripperSlipServoCommandConstPtr& msg)
00710 {
00711 if(myPressureObserver->broken_sensor)
00712 {
00713 ROS_ERROR("REFUSING TO SLIP SERVO - PRESSURE SENSOR HAS ZERO READING AND MAY BE MALFUNCTIONING!");
00714 return;
00715 }
00716
00717 reinitializeValues();
00718
00719
00720 if(fingertip_start_force <= 0.0)
00721 myGripperController->servoForce = fingertip_start_force;
00722 else
00723 myGripperController->servoForce = servo_force;
00724
00725
00726 control_mode = rt_state_def.SLIP_SERVO;
00727
00728 ROS_INFO("Starting Slip Servo with: %f N", myGripperController->servoForce);
00729 }
00730
00731
00741 void PR2GripperSensorController::forceServoCB(const pr2_gripper_sensor_msgs::PR2GripperForceServoCommandConstPtr& msg)
00742 {
00743 if(myPressureObserver->broken_sensor)
00744 {
00745 ROS_ERROR("REFUSING TO FORCE SERVO - PRESSURE SENSOR HAS ZERO READING AND MAY BE MALFUNCTIONING!");
00746 return;
00747 }
00748
00749 reinitializeValues();
00750
00751 if(msg->fingertip_force >= 0)
00752 servo_force = -(msg->fingertip_force);
00753 else
00754 servo_force = 0.0;
00755
00756
00757 control_mode = rt_state_def.FORCE_SERVO;
00758
00759 ROS_INFO("Starting Force Servo with: %f N", servo_force);
00760 }
00761
00762
00763
00764 PLUGINLIB_EXPORT_CLASS(pr2_gripper_sensor_controller::PR2GripperSensorController,pr2_controller_interface::Controller)