$search
00001 /********************************************************************* 00002 * 00003 * Software License Agreement (BSD License) 00004 * 00005 * Copyright (c) 2008, Willow Garage, Inc. 00006 * All rights reserved. 00007 * 00008 * Redistribution and use in source and binary forms, with or without 00009 * modification, are permitted provided that the following conditions 00010 * are met: 00011 * 00012 * * Redistributions of source code must retain the above copyright 00013 * notice, this list of conditions and the following disclaimer. 00014 * * Redistributions in binary form must reproduce the above 00015 * copyright notice, this list of conditions and the following 00016 * disclaimer in the documentation and/or other materials provided 00017 * with the distribution. 00018 * * Neither the name of the Willow Garage nor the names of its 00019 * contributors may be used to endorse or promote products derived 00020 * from this software without specific prior written permission. 00021 * 00022 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00023 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00024 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00025 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00026 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00027 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00028 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00029 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00030 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00031 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00032 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00033 * POSSIBILITY OF SUCH DAMAGE. 00034 * 00035 * \author Joe Romano 00036 *********************************************************************/ 00037 //@author Joe Romano 00038 //@email joeromano@gmail.com 00039 //@brief Controller to read data from the sensors on the pr2 gripper 00040 // (accelerometer in palm, pressure arrays on the fingers, and 00041 // encoder in the hand) and publish higher-level useful processed 00042 // information about them, as well as perform low-level control 00043 // tasks to control the gripper based on these signals in real-time. 00044 // 00045 // This controller is inteded to be interacted with through its 00046 // action interface, pr2_gripper_sensor_action. 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 // get a handle to the hardware interface 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 // get a handle to our accelerometer 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 // get a handle to our left finger pressure sensors 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 // get a handle to our right finger pressure sensors 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 // get a handle to our desired joint 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 // instantiate our gripper action object and pass it handles to our joint_state_controller and observer objects 00139 myGripperController = new gripperController(joint_state_, myPressureObserver, myAccelerationObserver); 00140 00141 return true; 00142 } 00143 00144 00156 bool PR2GripperSensorController::reloadParams(std_srvs::Empty::Request& req,std_srvs::Empty::Response& resp) 00157 { 00158 if(!nodeHandle.getParam("close_speed", servo_velocity)) 00159 ROS_ERROR("No close_speed given in namespace: '%s')", nodeHandle.getNamespace().c_str()); 00160 servo_velocity = -servo_velocity; 00161 if(servo_velocity > 0) 00162 { 00163 ROS_ERROR("Incorrect sign on close_speed (negative speed is impossible), setting to 0!"); 00164 servo_velocity = 0; 00165 } 00166 00167 if(!nodeHandle.getParam("max_joint_effort", max_effort)) 00168 ROS_ERROR("No max_joint_effort given in namespace: '%s')", nodeHandle.getNamespace().c_str()); 00169 myGripperController->max_effort = max_effort; 00170 00171 if(!nodeHandle.getParam("fingertip_force_limit", fingertip_force_limit)) 00172 ROS_ERROR("No fingertip_force_limit given in namespace: '%s')", nodeHandle.getNamespace().c_str()); 00173 fingertip_force_limit = -fingertip_force_limit; 00174 myGripperController->fingertip_force_limit = fingertip_force_limit; 00175 00176 if(!nodeHandle.getParam("deformation_limit", deformation_limit)) 00177 ROS_ERROR("No deformation_limit given in namespace: '%s')", nodeHandle.getNamespace().c_str()); 00178 myGripperController->positionMarker_limit = deformation_limit; 00179 00180 if(!nodeHandle.getParam("slip_servo_start_force", fingertip_start_force)) 00181 ROS_ERROR("No slip_servo_start_force given in namespace: '%s')", nodeHandle.getNamespace().c_str()); 00182 fingertip_start_force = -fingertip_start_force; 00183 00184 if(!nodeHandle.getParam("force_lightest", myPressureObserver->forceLightest)) 00185 ROS_ERROR("No force_lightest given in namespace: '%s')", nodeHandle.getNamespace().c_str()); 00186 00187 if(!nodeHandle.getParam("hp_force_trigger", myPressureObserver->hpForceTrigger)) 00188 ROS_ERROR("No hpForceTrigger given in namespace: '%s')", nodeHandle.getNamespace().c_str()); 00189 00190 if(!nodeHandle.getParam("force_servo_force_tolerance", myGripperController->force_servo_force_tolerance)) 00191 ROS_ERROR("No force_servo_force_tolerance given in namespace: '%s')", nodeHandle.getNamespace().c_str()); 00192 00193 if(!nodeHandle.getParam("force_servo_velocity_tolerance", force_servo_velocity_tolerance)) 00194 ROS_ERROR("No force_servo_velocity_tolerance given in namespace: '%s')", nodeHandle.getNamespace().c_str()); 00195 00196 if(!nodeHandle.getParam("position_servo_position_tolerance", force_servo_velocity_tolerance)) 00197 ROS_ERROR("No position_servo_position_tolerance given in namespace: '%s')", nodeHandle.getNamespace().c_str()); 00198 00199 00200 return true; 00201 00202 } 00203 00204 00205 00219 bool PR2GripperSensorController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n) 00220 { 00221 // make the below true if you want to publish raw data messages (debug mode) 00222 publish_raw_data = false; 00223 00224 // store the node and robot handle 00225 assert(robot); 00226 nodeHandle = n; 00227 robotHandle = robot; 00228 00229 // initialize handles to our sensors and joints 00230 bool success = this->initializeHandles(robot, n); 00231 loop_count_ = 0; 00232 00233 // initially place our controller into disabled mode 00234 control_mode = rt_state_def.DISABLED; 00235 00236 // seed the timer 00237 last_time_ = robot->getTime(); 00238 00239 //publish our services 00240 updateZeros_srv_ = n.advertiseService("zero_fingertip_sensors", &PR2GripperSensorController::updateZeros, this); 00241 reloadParams_srv_ = n.advertiseService("reload_params", &PR2GripperSensorController::reloadParams, this); 00242 stopMotorOutput_srv_ = n.advertiseService("stop_motor_output", &PR2GripperSensorController::stopMotorOutput, this); 00243 00244 //setup our publisher for raw data information (only if flag is set) 00245 if(publish_raw_data) 00246 { 00247 raw_data_publisher_.reset(new realtime_tools::RealtimePublisher<pr2_gripper_sensor_msgs::PR2GripperSensorRawData>(n, "raw_data", 1)); 00248 } 00249 00250 //setup our publisher for joint and motor effort state information 00251 controller_state_publisher_.reset(new realtime_tools::RealtimePublisher<pr2_controllers_msgs::JointControllerState>(n, "state", 1)); 00252 00253 //setup our publisher for contact state information 00254 contact_state_publisher_.reset(new realtime_tools::RealtimePublisher<pr2_gripper_sensor_msgs::PR2GripperFindContactData>(n, "contact_state", 1)); 00255 00256 //setup our publisher for place state information 00257 event_detector_state_publisher_.reset(new realtime_tools::RealtimePublisher<pr2_gripper_sensor_msgs::PR2GripperEventDetectorData>(n, "event_detector_state", 1)); 00258 00259 //setup our publisher for slip state information 00260 slip_state_publisher_.reset(new realtime_tools::RealtimePublisher<pr2_gripper_sensor_msgs::PR2GripperSlipServoData>(n, "slip_servo_state", 1)); 00261 00262 //setup our publisher for force state information 00263 force_state_publisher_.reset(new realtime_tools::RealtimePublisher<pr2_gripper_sensor_msgs::PR2GripperForceServoData>(n, "force_servo_state", 1)); 00264 00265 // setup our callback to handle position command requests 00266 sub_command_ = n.subscribe<pr2_controllers_msgs::Pr2GripperCommand>("command", 1, &PR2GripperSensorController::positionCB, this); 00267 00268 // setup our callback to handle find_contact command requests 00269 sub_findcontact_command_ = n.subscribe<pr2_gripper_sensor_msgs::PR2GripperFindContactCommand>("find_contact", 1, &PR2GripperSensorController::findContactCB, this); 00270 00271 // setup our callback to handle event_detector command requests 00272 sub_event_detector_command_ = n.subscribe<pr2_gripper_sensor_msgs::PR2GripperEventDetectorCommand>("event_detector", 1, &PR2GripperSensorController::eventDetectorCB, this); 00273 00274 // setup our callback to handle slip_servo command requests 00275 sub_slipservo_command_ = n.subscribe<pr2_gripper_sensor_msgs::PR2GripperSlipServoCommand>("slip_servo", 1, &PR2GripperSensorController::slipServoCB, this); 00276 00277 // setup our callback to handle force_servo command requests 00278 sub_forceservo_command_ = n.subscribe<pr2_gripper_sensor_msgs::PR2GripperForceServoCommand>("force_servo", 1, &PR2GripperSensorController::forceServoCB, this); 00279 00280 // upate the zero offset of our pressure sensors 00281 std_srvs::Empty::Request req; 00282 std_srvs::Empty::Response rsp; 00283 updateZeros(req,rsp); 00284 00285 // load our params from the param server 00286 reloadParams(req,rsp); 00287 00288 return success; 00289 00290 } 00291 00292 00293 00302 void PR2GripperSensorController::update() 00303 { 00304 00305 // update our timing information 00306 ros::Time time = robotHandle->getTime(); 00307 ros::Duration dt = time - last_time_; 00308 00309 // do some sanity checks to see that we have a valid robot handle 00310 assert(robotHandle != NULL); 00311 00312 //---------------------------------------- 00313 // Begin State Machine 00314 00315 //disabled (do nothing) mode 00316 if(control_mode == rt_state_def.DISABLED){} 00317 00318 // position control mode 00319 else if(control_mode == rt_state_def.POSITION_SERVO) 00320 { 00321 // use a zero velocity goal in our position controller to have the robot 00322 // gently decelerate when the position error gets small (could be current vel. instead) 00323 myGripperController->positionServo(servo_position,0.0); 00324 // keep moving our posiiton marker to avoid deformation_limit errors 00325 myGripperController->positionMarker = myGripperController->positionCurrent; 00326 } 00327 00328 // force control mode 00329 else if(control_mode == rt_state_def.FORCE_SERVO) 00330 { 00331 //if we have reached some epsilon of our target force 00332 // and the gripper is still then set consider the force contact stable 00333 if( ( (myGripperController->forceServo2(servo_force)) && 00334 fabs(myGripperController->lpVelocity) < force_servo_velocity_tolerance)) 00335 00336 { 00337 // require the same force for 250ms 00338 if(findContact_delay > 250) 00339 stable_force = true; 00340 findContact_delay++; 00341 } 00342 } 00343 00344 // find contact mode 00345 else if(control_mode == rt_state_def.FIND_CONTACT) 00346 { 00347 // if we have not yet met the contact requirements 00348 if(!myGripperController->grabObject(servo_velocity, contacts_desired)) 00349 { 00350 contact_success = false; 00351 // make sure our velocity integral term is starting at our current position in case of contact 00352 myGripperController->vel_integral = myGripperController->gripper_state_now_position; 00353 // keep moving our posiiton marker to avoid deformation_limit errors 00354 myGripperController->positionMarker = myGripperController->positionCurrent; 00355 } 00356 // else if we have met the contact requirements 00357 else 00358 { 00359 00360 // for 50 ms after contact 00361 if(contactCounter < 50) 00362 { 00363 servo_position = myGripperController->positionContact; 00364 00365 // keep track of the highest contact force we've applied 00366 servo_force = myGripperController->forceContact; 00367 00368 // hold our position 00369 myGripperController->positionServo(servo_position,0.0); 00370 } 00371 00372 else 00373 { 00374 myGripperController->positionServo(servo_position,0.0); 00375 stable_contact = true; 00376 control_mode = rt_state_def.POSITION_SERVO; 00377 } 00378 00379 contactCounter++; 00380 contact_success = true; 00381 } 00382 } 00383 00384 // if in slip servo control mode 00385 else if(control_mode == rt_state_def.SLIP_SERVO) 00386 myGripperController->slipServo2(); 00387 00388 00389 // else somehow we ended up with some other value for mode (should not be possible) 00390 else 00391 { 00392 ROS_ERROR("PR2_GRIPPER_SENSOR_CONTROLLER ENTERED A BAD STATE MACHINE STATE"); 00393 myGripperController->jointState->commanded_effort_ = 0; 00394 // try to recover by setting the control mode to "disabled" 00395 control_mode = rt_state_def.DISABLED; 00396 } 00397 00398 // End State Machine 00399 //---------------------------------------- 00400 00401 //---------------------------------------- 00402 // Non-state machine controller updating 00403 00404 // update our placement variables 00405 if(myGripperController->place(placeConditions,acc_trigger,slip_trigger)) 00406 placedState = true; 00407 00408 // update zeros mode 00409 if(update_zeros == true) 00410 myPressureObserver->updateZeros2(); 00411 00412 //---------------------------------------- 00413 // Begin Feedback publication 00414 00415 // publish information every nth cycle 00416 if(loop_count_ % 1 == 0) 00417 { 00418 if(controller_state_publisher_ && controller_state_publisher_->trylock()) 00419 { 00420 controller_state_publisher_->msg_.header.stamp = time; 00421 controller_state_publisher_->msg_.set_point = servo_position; 00422 controller_state_publisher_->msg_.process_value = myGripperController->gripper_state_now_position; 00423 controller_state_publisher_->msg_.process_value_dot = myGripperController->gripper_state_now_velocity; 00424 controller_state_publisher_->msg_.error = myGripperController->gripper_state_now_position - servo_position; 00425 controller_state_publisher_->msg_.time_step = dt.toSec(); 00426 controller_state_publisher_->msg_.command = myGripperController->gripper_state_now_measured_effort; 00427 00428 controller_state_publisher_->unlockAndPublish(); 00429 } 00430 00431 if(contact_state_publisher_ && contact_state_publisher_->trylock()) 00432 { 00433 contact_state_publisher_->msg_.stamp = time; 00434 contact_state_publisher_->msg_.left_fingertip_pad_contact = myPressureObserver->left_contact; 00435 contact_state_publisher_->msg_.right_fingertip_pad_contact = myPressureObserver->right_contact; 00436 contact_state_publisher_->msg_.left_fingertip_pad_force = myPressureObserver->padForce_left_cur_nonbiased; 00437 contact_state_publisher_->msg_.right_fingertip_pad_force = myPressureObserver->padForce_right_cur_nonbiased; 00438 contact_state_publisher_->msg_.joint_position = myGripperController->positionContact; 00439 contact_state_publisher_->msg_.joint_effort = myGripperController->gripper_state_now_measured_effort; 00440 contact_state_publisher_->msg_.rtstate.realtime_controller_state = control_mode; 00441 contact_state_publisher_->msg_.contact_conditions_met = stable_contact; 00442 00443 00444 contact_state_publisher_->unlockAndPublish(); 00445 } 00446 00447 if(slip_state_publisher_ && slip_state_publisher_->trylock()) 00448 { 00449 slip_state_publisher_->msg_.stamp = time; 00450 slip_state_publisher_->msg_.joint_effort = myGripperController->gripper_state_now_measured_effort; 00451 slip_state_publisher_->msg_.left_fingertip_pad_force = myPressureObserver->padForce_left_cur_nonbiased; 00452 slip_state_publisher_->msg_.right_fingertip_pad_force = myPressureObserver->padForce_right_cur_nonbiased; 00453 slip_state_publisher_->msg_.deformation = myGripperController->positionCurrent - myGripperController->positionMarker; 00454 slip_state_publisher_->msg_.slip_detected = myGripperController->slip_flag; 00455 slip_state_publisher_->msg_.fingertip_force_limit_reached = myGripperController->force_limit_flag; 00456 slip_state_publisher_->msg_.deformation_limit_reached = myGripperController->deformation_limit_flag; 00457 slip_state_publisher_->msg_.gripper_empty = myGripperController->dropped_flag; 00458 slip_state_publisher_->msg_.rtstate.realtime_controller_state = control_mode; 00459 00460 slip_state_publisher_->unlockAndPublish(); 00461 } 00462 00463 if(force_state_publisher_ && force_state_publisher_->trylock()) 00464 { 00465 force_state_publisher_->msg_.stamp = time; 00466 force_state_publisher_->msg_.joint_effort = myGripperController->gripper_state_now_measured_effort; 00467 force_state_publisher_->msg_.left_fingertip_pad_force = myPressureObserver->padForce_left_cur_nonbiased; 00468 force_state_publisher_->msg_.right_fingertip_pad_force = myPressureObserver->padForce_right_cur_nonbiased; 00469 force_state_publisher_->msg_.force_achieved = stable_force; 00470 force_state_publisher_->msg_.rtstate.realtime_controller_state = control_mode; 00471 00472 force_state_publisher_->unlockAndPublish(); 00473 } 00474 00475 if(event_detector_state_publisher_ && event_detector_state_publisher_->trylock()) 00476 { 00477 event_detector_state_publisher_->msg_.stamp = time; 00478 event_detector_state_publisher_->msg_.trigger_conditions_met = placedState; 00479 event_detector_state_publisher_->msg_.slip_event = myPressureObserver->placeContact; 00480 event_detector_state_publisher_->msg_.acceleration_event = myAccelerationObserver->placeContact; 00481 event_detector_state_publisher_->msg_.acceleration_vector[0] = myAccelerationObserver->aX_bp; 00482 event_detector_state_publisher_->msg_.acceleration_vector[1] = myAccelerationObserver->aY_bp; 00483 event_detector_state_publisher_->msg_.acceleration_vector[2] = myAccelerationObserver->aZ_bp; 00484 00485 event_detector_state_publisher_->unlockAndPublish(); 00486 } 00487 } 00488 00489 // publish information every nth cycle 00490 if(loop_count_ % 1 == 0) 00491 { 00492 if(publish_raw_data) 00493 { 00494 raw_data_publisher_->msg_.stamp = time; 00495 raw_data_publisher_->msg_.left_finger_pad_force = myPressureObserver->padForce_left_cur_nonbiased; 00496 raw_data_publisher_->msg_.right_finger_pad_force = myPressureObserver->padForce_right_cur_nonbiased; 00497 raw_data_publisher_->msg_.left_finger_pad_force_filtered = myPressureObserver->padForce_left_cur; 00498 raw_data_publisher_->msg_.right_finger_pad_force_filtered = myPressureObserver->padForce_right_cur; 00499 raw_data_publisher_->msg_.left_finger_pad_forces = myPressureObserver->pressure_current_zerod.pressure_left; 00500 raw_data_publisher_->msg_.right_finger_pad_forces = myPressureObserver->pressure_current_zerod.pressure_right; 00501 raw_data_publisher_->msg_.left_finger_pad_forces_filtered = myPressureObserver->pressure_cur_bias.pressure_left; 00502 raw_data_publisher_->msg_.right_finger_pad_forces_filtered = myPressureObserver->pressure_cur_bias.pressure_right; 00503 raw_data_publisher_->msg_.acc_x_raw = myAccelerationObserver->aX_lp; 00504 raw_data_publisher_->msg_.acc_y_raw = myAccelerationObserver->aY_lp; 00505 raw_data_publisher_->msg_.acc_z_raw = myAccelerationObserver->aZ_lp; 00506 raw_data_publisher_->msg_.acc_x_filtered = myAccelerationObserver->aX_bp; 00507 raw_data_publisher_->msg_.acc_y_filtered = myAccelerationObserver->aY_bp; 00508 raw_data_publisher_->msg_.acc_z_filtered = myAccelerationObserver->aZ_bp; 00509 raw_data_publisher_->msg_.left_contact = myPressureObserver->left_contact; 00510 raw_data_publisher_->msg_.right_contact = myPressureObserver->right_contact; 00511 00512 raw_data_publisher_->unlockAndPublish(); 00513 } 00514 } 00515 00516 // End Feedback publication 00517 //---------------------------------------- 00518 00519 // update our gripper action, as well as our accelerometer and pressure states with it 00520 myGripperController->updateData(); 00521 00522 // increment our counter and store the time 00523 loop_count_++; 00524 last_time_ = time; 00525 } 00526 00533 void PR2GripperSensorController::stopping() 00534 { 00535 // turn the motors off before stopping 00536 myGripperController->jointState->commanded_effort_ = 0; 00537 } 00538 00539 00540 00554 bool PR2GripperSensorController::updateZeros(std_srvs::Empty::Request& req,std_srvs::Empty::Response& resp) 00555 { 00556 ROS_INFO("Updating zeros...."); 00557 update_zeros = true; 00558 ros::Duration(0.25).sleep(); 00559 update_zeros = false; 00560 ROS_INFO(".... zeros finished updating"); 00561 00562 return true; 00563 } 00564 00565 00578 bool PR2GripperSensorController::stopMotorOutput(std_srvs::Empty::Request& req,std_srvs::Empty::Response& resp) 00579 { 00580 ROS_INFO("Stopping gripper command output."); 00581 control_mode = rt_state_def.DISABLED; 00582 00583 return true; 00584 } 00585 00586 00591 void PR2GripperSensorController::reinitializeValues() 00592 { 00593 findContact_delay = 0; 00594 stable_force = false; 00595 00596 // reinitialize force ramp state 00597 myGripperController->firstRamp = true; 00598 00599 myGripperController->vel_integral_vcontrol = myGripperController->positionCurrent; 00600 00601 // reinialize the grasp contact states 00602 stable_contact = false; 00603 00604 // reinitialize the contact forces 00605 myGripperController->forceContact = 0; 00606 myGripperController->forceContact_l = 0; 00607 myGripperController->forceContact_r = 0; 00608 00609 // reset several important variables 00610 myGripperController->positionMarker = myGripperController->positionCurrent; 00611 myGripperController->slip_flag = false; 00612 myGripperController->force_limit_flag = false; 00613 myGripperController->deformation_limit_flag = false; 00614 00615 myGripperController->dropped_flag = false; 00616 00617 } 00618 00619 00628 void PR2GripperSensorController::positionCB(const pr2_controllers_msgs::Pr2GripperCommandConstPtr& msg) 00629 { 00630 reinitializeValues(); 00631 00632 // store the message data 00633 servo_position = msg->position; 00634 max_effort = msg->max_effort; 00635 myGripperController->max_effort = max_effort; 00636 00637 00638 // put the max effort value commanded by the user back on the param server 00639 nodeHandle.setParam("max_joint_effort", max_effort); 00640 00641 00642 // change the controller state 00643 control_mode = rt_state_def.POSITION_SERVO; 00644 00645 ROS_INFO("Gripper Position Servo to: %f", servo_position); 00646 } 00647 00648 00657 void PR2GripperSensorController::findContactCB(const pr2_gripper_sensor_msgs::PR2GripperFindContactCommandConstPtr& msg) 00658 { 00659 if(myPressureObserver->broken_sensor) 00660 { 00661 ROS_ERROR("REFUSING TO FIND CONTACT - PRESSURE SENSOR HAS ZERO READING AND MAY BE MALFUNCTIONING!"); 00662 return; 00663 } 00664 00665 reinitializeValues(); 00666 contactCounter = 0; 00667 00668 // copy over the data from our message 00669 contacts_desired = msg->contact_conditions; 00670 00671 // reinialize the grasp contact states 00672 myPressureObserver->left_contact = false; 00673 myPressureObserver->right_contact = false; 00674 00675 // change the controller state 00676 control_mode = rt_state_def.FIND_CONTACT; 00677 00678 } 00679 00680 00689 void PR2GripperSensorController::eventDetectorCB(const pr2_gripper_sensor_msgs::PR2GripperEventDetectorCommandConstPtr& msg) 00690 { 00691 placeConditions = msg->trigger_conditions; 00692 acc_trigger = msg->acceleration_trigger_magnitude; 00693 slip_trigger = msg->slip_trigger_magnitude; 00694 00695 placedState = false; 00696 myPressureObserver->placeContact = false; 00697 myAccelerationObserver->placeContact = false; 00698 } 00699 00707 void PR2GripperSensorController::slipServoCB(const pr2_gripper_sensor_msgs::PR2GripperSlipServoCommandConstPtr& msg) 00708 { 00709 if(myPressureObserver->broken_sensor) 00710 { 00711 ROS_ERROR("REFUSING TO SLIP SERVO - PRESSURE SENSOR HAS ZERO READING AND MAY BE MALFUNCTIONING!"); 00712 return; 00713 } 00714 00715 reinitializeValues(); 00716 00717 // check if the user wanted to use the default force or input a force level 00718 if(fingertip_start_force <= 0.0) 00719 myGripperController->servoForce = fingertip_start_force; 00720 else 00721 myGripperController->servoForce = servo_force; 00722 00723 // change the controller state 00724 control_mode = rt_state_def.SLIP_SERVO; 00725 00726 ROS_INFO("Starting Slip Servo with: %f N", myGripperController->servoForce); 00727 } 00728 00729 00739 void PR2GripperSensorController::forceServoCB(const pr2_gripper_sensor_msgs::PR2GripperForceServoCommandConstPtr& msg) 00740 { 00741 if(myPressureObserver->broken_sensor) 00742 { 00743 ROS_ERROR("REFUSING TO FORCE SERVO - PRESSURE SENSOR HAS ZERO READING AND MAY BE MALFUNCTIONING!"); 00744 return; 00745 } 00746 00747 reinitializeValues(); 00748 00749 if(msg->fingertip_force >= 0) 00750 servo_force = -(msg->fingertip_force); 00751 else 00752 servo_force = 0.0; 00753 00754 // change the controller state 00755 control_mode = rt_state_def.FORCE_SERVO; 00756 00757 ROS_INFO("Starting Force Servo with: %f N", servo_force); 00758 } 00759 00760 00761 // Register controller to pluginlib 00762 PLUGINLIB_DECLARE_CLASS(pr2_gripper_sensor_controller,PR2GripperSensorController, 00763 pr2_gripper_sensor_controller::PR2GripperSensorController, 00764 pr2_controller_interface::Controller)