// Close end effector to setpoint bool setEndEffector(double setpoint) { // Error check - servos are alive and we've been recieving messages if( !endEffectorResponding() ) { return false; } // Check that there is a valid end effector setpoint set if( setpoint >= END_EFFECTOR_CLOSE_VALUE_MAX && setpoint <= END_EFFECTOR_OPEN_VALUE_MAX ) { ROS_ERROR_STREAM_NAMED("clam_gripper_controller","Unable to set end effector: out of range setpoint of " << setpoint << ". Valid range is " << END_EFFECTOR_CLOSE_VALUE_MAX << " to " << END_EFFECTOR_OPEN_VALUE_MAX ); return false; } // Check if end effector is already close and arm is still if( ee_status_.target_position == setpoint && ee_status_.moving == false && ee_status_.position > setpoint + END_EFFECTOR_POSITION_TOLERANCE && ee_status_.position < setpoint - END_EFFECTOR_POSITION_TOLERANCE ) { // Consider the ee to already be in the corret position ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","End effector close: already in position"); return true; } // Publish command to servos std_msgs::Float64 joint_value; joint_value.data = setpoint; end_effector_pub_.publish(joint_value); // Wait until end effector is done moving int timeout = 0; while( ee_status_.moving == true && ee_status_.position > setpoint + END_EFFECTOR_POSITION_TOLERANCE && ee_status_.position < setpoint - END_EFFECTOR_POSITION_TOLERANCE && ros::ok() ) { // Feedback feedback_.position = ee_status_.position; //TODO: fill in more of the feedback action_server_->publishFeedback(feedback_); ros::Duration(0.25).sleep(); ++timeout; if( timeout > 16 ) // wait 4 seconds { ROS_ERROR_NAMED("clam_gripper_controller","Unable to close end effector: timeout on goal position"); return false; } } return true; }
// Close end effector bool closeEndEffector() { // Error check - servos are alive and we've been recieving messages if( !endEffectorResponding() ) { return false; } // Check if end effector is already close and arm is stil if( ee_status_.target_position == END_EFFECTOR_CLOSE_VALUE_MAX && ee_status_.moving == false && ee_status_.position > END_EFFECTOR_CLOSE_VALUE_MAX - END_EFFECTOR_POSITION_TOLERANCE && ee_status_.position < END_EFFECTOR_CLOSE_VALUE_MAX + END_EFFECTOR_POSITION_TOLERANCE ) { // Consider the ee to already be in the correct position ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","End effector already closed within tolerance, unable to close further"); return true; } // Set the velocity for the end effector to a low value ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","Setting end effector servo velocity low"); velocity_client_ = nh_.serviceClient< dynamixel_hardware_interface::SetVelocity >(EE_VELOCITY_SRV_NAME); if( !velocity_client_.waitForExistence(ros::Duration(5.0)) ) { ROS_ERROR_NAMED("clam_gripper_controller","Timed out waiting for velocity client existance"); return false; } dynamixel_hardware_interface::SetVelocity set_velocity_srv; set_velocity_srv.request.velocity = END_EFFECTOR_MEDIUM_VELOCITY; if( !velocity_client_.call(set_velocity_srv) ) { ROS_ERROR_NAMED("clam_gripper_controller","Failed to set the end effector servo velocity via service call"); return false; } std_msgs::Float64 joint_value; double timeout_sec = 10.0; // time before we declare an error const double CHECK_INTERVAL = 0.1; // how often we check the load const double FIRST_BACKOUT_AMOUNT = -0.25; const double SECOND_BACKOUT_AMOUNT = -0.0075; const double BACKOUT_AMOUNT[2] = {FIRST_BACKOUT_AMOUNT, SECOND_BACKOUT_AMOUNT}; // Grasp twice - to reduce amount of slips and ensure better grip for(int i = 0; i < 2; ++i) { timeout_sec = 10; // reset timeout; ROS_DEBUG_STREAM("Grasping with end effector - grasp number " << i + 1); // Tell servos to start closing slowly to max amount joint_value.data = END_EFFECTOR_CLOSE_VALUE_MAX; end_effector_pub_.publish(joint_value); // Wait until end effector is done moving while( ee_status_.position < joint_value.data - END_EFFECTOR_POSITION_TOLERANCE || ee_status_.position > joint_value.data + END_EFFECTOR_POSITION_TOLERANCE ) { ros::spinOnce(); // Allows ros to get the latest servo message - we need the load // Check if load has peaked if( ee_status_.load < END_EFFECTOR_LOAD_SETPOINT ) // we have touched object! { // Open the gripper back up a little to reduce the amount of load. // the first time open it a lot to help with grasp quality joint_value.data = ee_status_.position + BACKOUT_AMOUNT[i]; // Check that we haven't passed the open limit if( joint_value.data < END_EFFECTOR_OPEN_VALUE_MAX ) joint_value.data = END_EFFECTOR_OPEN_VALUE_MAX; ROS_DEBUG_NAMED("clam_gripper_controller","Setting end effector setpoint to %f when it was %f", joint_value.data, ee_status_.position); end_effector_pub_.publish(joint_value); if( i == 0 ) // give lots of time to pull out the first time { ros::Duration(1.00).sleep(); ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","Sleeping as we publish joint value " << joint_value.data); set_velocity_srv.request.velocity = END_EFFECTOR_SLOW_VELOCITY; if( !velocity_client_.call(set_velocity_srv) ) { ROS_ERROR_NAMED("clam_gripper_controller","Failed to set the end effector servo velocity via service call"); return false; } ros::Duration(1.0).sleep(); } break; } // Debug output ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","" << joint_value.data - END_EFFECTOR_POSITION_TOLERANCE << " < " << ee_status_.position << " < " << joint_value.data + END_EFFECTOR_POSITION_TOLERANCE << " -- LOAD: " << ee_status_.load ); // Feedback feedback_.position = ee_status_.position; //TODO: fill in more of the feedback action_server_->publishFeedback(feedback_); // Wait an interval before checking again ros::Duration(CHECK_INTERVAL).sleep(); // Check if timeout has occured timeout_sec -= CHECK_INTERVAL; if( timeout_sec <= 0 ) { ROS_ERROR_NAMED("clam_gripper_controller","Timeout: Unable to close end effector"); return false; } } } // DONE ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","Finished closing end effector action"); return true; }
// Open end effector bool openEndEffector() { // Error check - servos are alive and we've been recieving messages if( !endEffectorResponding() ) { return false; } // Check if end effector is already open and arm is still if( ee_status_.target_position == END_EFFECTOR_OPEN_VALUE_MAX && ee_status_.moving == false && ee_status_.position > END_EFFECTOR_OPEN_VALUE_MAX + END_EFFECTOR_POSITION_TOLERANCE && ee_status_.position < END_EFFECTOR_OPEN_VALUE_MAX - END_EFFECTOR_POSITION_TOLERANCE ) { // Consider the ee to already be in the corret position ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","End effector open: already in position"); return true; } // Set the velocity for the end effector servo ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","Setting end effector servo velocity"); velocity_client_ = nh_.serviceClient< dynamixel_hardware_interface::SetVelocity >(EE_VELOCITY_SRV_NAME); while(!velocity_client_.waitForExistence(ros::Duration(10.0))) { ROS_ERROR_STREAM_NAMED("clam_gripper_controller","Failed to set the end effector servo velocity via service call"); return false; } dynamixel_hardware_interface::SetVelocity set_velocity_srv; set_velocity_srv.request.velocity = END_EFFECTOR_VELOCITY; if( !velocity_client_.call(set_velocity_srv) ) { ROS_ERROR_STREAM_NAMED("clam_gripper_controller","Failed to set the end effector servo velocity via service call"); return false; } // Publish command to servos std_msgs::Float64 joint_value; joint_value.data = END_EFFECTOR_OPEN_VALUE_MAX; end_effector_pub_.publish(joint_value); // Wait until end effector is done moving int timeout = 0; while( ee_status_.moving == true && ee_status_.position > END_EFFECTOR_OPEN_VALUE_MAX + END_EFFECTOR_POSITION_TOLERANCE && ee_status_.position < END_EFFECTOR_OPEN_VALUE_MAX - END_EFFECTOR_POSITION_TOLERANCE && ros::ok() ) { // Feedback feedback_.position = ee_status_.position; //TODO: fill in more of the feedback action_server_->publishFeedback(feedback_); // Looping ros::Duration(0.25).sleep(); ++timeout; if( timeout > 16 ) // wait 4 seconds { ROS_ERROR_NAMED("clam_gripper_controller","Unable to open end effector: timeout on goal position"); return false; } } // It worked! ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","Finished end effector action"); return true; }