def __init__(self, side="right", calibrate=True):
"""
Constructor.
@type side: str
@param side: robot gripper name
@type calibrate: bool
@param calibrate: Attempts to calibrate the gripper when initializing class (defaults True)
"""
self.devices = None
self.name = '_'.join([side, 'gripper'])
self.ee_config_sub = rospy.Subscriber('/io/end_effector/config',
IONodeConfiguration,
self._config_callback)
# Wait for the gripper device status to be true
intera_dataflow.wait_for(
lambda: not self.devices is None,
timeout=5.0,
timeout_msg=(
"Failed to get gripper. No gripper attached on the robot."))
self.gripper_io = IODeviceInterface("end_effector", self.name)
if self.has_error():
self.reboot()
calibrate = True
if calibrate and self.is_calibrated():
self.calibrate()
def __init__(self, ee_name="right_gripper", calibrate=True):
"""
Constructor.
@type name: str
@param name: robot gripper name (default: "right_gripper")
@type calibrate: bool
@param calibrate: Attempts to calibrate the gripper when initializing class (defaults True)
"""
self.devices = None
self.name = ee_name
if ee_name == 'right' or ee_name == 'left':
rospy.logwarn((
"Specifying gripper by 'side' is deprecated, please use full name"
" (ex: 'right_gripper')."))
self.name = '_'.join([ee_name, 'gripper'])
self.ee_config_sub = rospy.Subscriber('/io/end_effector/config',
IONodeConfiguration,
self._config_callback)
# Wait for the gripper device status to be true
intera_dataflow.wait_for(
lambda: not self.devices is None,
timeout=5.0,
timeout_msg=(
"Failed to get gripper. No gripper attached on the robot."))
self.gripper_io = IODeviceInterface("end_effector", self.name)
if self.has_error():
self.reboot()
calibrate = True
if calibrate and not self.is_calibrated():
self.calibrate()
def __init__(self, ee_device_id, initialize=True):
self.name = ee_device_id
self.endpoint_map = None
self.gripper_io = None
self._node_state = None
self._node_device_status = None
self._node_command_pub = rospy.Publisher('io/end_effector/command',
IOComponentCommand,
queue_size=10)
self._node_state_sub = rospy.Subscriber('io/end_effector/state',
IONodeStatus,
self._node_state_cb)
# Wait for the gripper device status to be true
intera_dataflow.wait_for(
lambda: self._node_device_status is not None,
timeout=5.0,
raise_on_error=initialize,
timeout_msg=(
"Failed to get gripper. No gripper attached on the robot."))
self.gripper_io = IODeviceInterface("end_effector", self.name)
self.gripper_io.config_changed.connect(self._load_endpoint_info)
if self.gripper_io.is_config_valid():
self._load_endpoint_info()
if initialize and self.needs_init():
self.initialize()
def __init__(self, limb="right"):
"""
Constructor.
@type limb: str
@param limb: limb side to interface
"""
params = RobotParams()
limb_names = params.get_limb_names()
if limb not in limb_names:
rospy.logerr("Cannot detect Cuff's limb {0} on this robot."
" Valid limbs are {1}. Exiting Cuff.init().".format(
limb, limb_names))
return
self.limb = limb
self.device = None
self.name = "cuff"
self.cuff_config_sub = rospy.Subscriber('/io/robot/cuff/config',
IODeviceConfiguration,
self._config_callback)
# Wait for the cuff status to be true
intera_dataflow.wait_for(
lambda: not self.device is None,
timeout=5.0,
timeout_msg=("Failed find cuff on limb '{0}'.".format(limb)))
self._cuff_io = IODeviceInterface("robot", self.name)
def __init__(self):
"""
Constructor.
"""
self._navigator_io = IODeviceInterface("robot", "navigator")
self._button_lookup = {
0: 'OFF',
1: 'CLICK',
2: 'LONG_PRESS',
3: 'DOUBLE_CLICK'
}
class Lights(object):
"""
Interface class for the lights on the Intera robots.
"""
def __init__(self):
"""
Constructor.
"""
self._lights_io = IODeviceInterface("robot", "robot")
def list_all_lights(self):
"""
Returns a list of strings describing all available lights
@rtype: list [str]
@return: a list of string representing light names
Each light name of the following format:
'<assembly>_<color>_light'
"""
return [
name for name in self._lights_io.list_signal_names()
if "light" in name
]
def set_light_state(self, name, on=True):
"""
Sets the named light the desired state (on=True, off=False)
@type name: str
@param name: Light name of the following format:
'<assembly>_<color>_light'
@type on: bool
@param on: value to set the light (on=True, off=False)
@rtype: bool
@return: True if the light state is set, False otherwise
"""
return self._lights_io.set_signal_value(name, on)
def get_light_state(self, name):
"""
Returns a boolean describing whether the requested light is 'ON'.
(True: on, False: off)
@type name: str
@param name: Light name of the following format:
'<assembly>_<color>_light'
@rtype: bool
@return: a variable representing light state: (True: on, False: off)
"""
return self._lights_io.get_signal_value(name)
def __init__(self):
"""
Constructor.
"""
camera_param_dict = RobotParams().get_camera_details()
camera_list = camera_param_dict.keys()
# check to make sure cameras is not an empty list
if not camera_list:
rospy.logerr(' '.join(
["camera list is empty: ", ' , '.join(camera_list)]))
return
camera_color_dict = {"mono": ['cognex'], "color": ['ienso_ethernet']}
self.cameras_io = dict()
for camera in camera_list:
if camera_param_dict[camera]['cameraType'] in camera_color_dict[
''
'color']:
is_color = True
else:
is_color = False
self.cameras_io[camera] = {
'interface': IODeviceInterface("internal"
"_camera", camera),
'is_color': is_color
}
def __init__(self):
"""
Constructor.
"""
self._node_config = None
self._node_config_sub = rospy.Subscriber('io/internal_camera/config',
IONodeConfiguration,
self._node_config_cb)
self.cameras_io = dict()
camera_param_dict = RobotParams().get_camera_details()
camera_list = camera_param_dict.keys()
# check to make sure cameras is not an empty list
if not camera_list:
rospy.logerr(' '.join(
["camera list is empty: ", ' , '.join(camera_list)]))
return
intera_dataflow.wait_for(
lambda: self._node_config is not None,
timeout=5.0,
timeout_msg=(
"Failed to connect to Camera Node and retrieve configuration."
))
cameras_to_load = self._get_camera_launch_config().keys()
camera_capabilities = {
"mono": ['cognex'],
"color": ['ienso_ethernet'],
"auto_exposure": ['ienso_ethernet'],
"auto_gain": ['ienso_ethernet']
}
for camera in camera_list:
cameraType = camera_param_dict[camera]['cameraType']
try:
interface = IODeviceInterface("internal_camera", camera)
self.cameras_io[camera] = {
'interface':
interface,
'is_color': (cameraType in camera_capabilities['color']),
'has_auto_exposure':
(cameraType in camera_capabilities['auto_exposure']),
'has_auto_gain': (cameraType
in camera_capabilities['auto_gain']),
}
except OSError as e:
if camera not in cameras_to_load:
rospy.logwarn(
"Expected camera ({0}) is not configured to launch in this run"
" configuration. Make sure you are running in SDK Mode."
.format(camera))
else:
rospy.logerr(
"Could not find expected camera ({0}) for this robot.\n"
"Please contact Rethink support: [email protected]"
.format(camera))
def _node_state_cb(self, msg):
# if we don't have a message yet, or if the timestamp is different: search for our device
if not self._node_state or IODeviceInterface.time_changed(
self._node_state.time, msg.time):
self._node_state = msg
if len(msg.devices) and msg.devices[0].name == self.name:
self._node_device_status = msg.devices[0].status
else:
# device removed
self._node_device_status = None
def __init__(self):
# let's create MoveIt! objects:
self.scene = moveit_commander.PlanningSceneInterface()
self.robot = moveit_commander.RobotCommander()
self.group = moveit_commander.MoveGroupCommander("right_arm")
self.group.set_max_velocity_scaling_factor(0.8)
self.group.set_max_acceleration_scaling_factor(0.8)
self.group.set_planning_time(60)
self.right_arm = ['right_j0', 'right_j1', 'right_j2', 'right_j3',
'right_j4', 'right_j5', 'right_j6']
rospy.loginfo("Initializing beerGrabber")
# ik service
self.ik_service_name = DEFAULT_IK_SERVICE
self.ik_serv = rospy.ServiceProxy(self.ik_service_name, SolvePositionIK)
self.ik_serv.wait_for_service()
rospy.loginfo("Successful connection to '" + self.ik_service_name + "'.")
# vision service
self.vision_service_name = DEFAULT_VISION_SERVICE
self.vision_serv = rospy.ServiceProxy(self.vision_service_name, target)
self.vision_serv.wait_for_service()
rospy.loginfo("Successful connection to '" + self.vision_service_name + "'.")
# check state service
self.check_service_name = DEFAULT_CHECK_SERVICE
# self.check_serv = rospy.ServiceProxy(self.check_service_name, GetStateValidity)
self.check_serv = rospy.ServiceProxy(self.check_service_name, GetStateValidity)
self.check_serv.wait_for_service()
rospy.loginfo("Successful connection to '" + self.check_service_name + "'.")
# initiate gripper
side="right"
grip_name = '_'.join([side, 'gripper'])
self.gripper_io = IODeviceInterface("end_effector", grip_name)
# add target position - might be useful when add bottle in the scene
self.target = PoseStamped()
self.dict = collisionObjects()
def grasp_server():
rospy.init_node('grasp_server', anonymous=True)
global arm
arm = Limb()
global group
group = moveit_commander.MoveGroupCommander("right_arm")
group.set_max_velocity_scaling_factor(0.0005)
group.set_max_acceleration_scaling_factor(0.0005)
group.set_planning_time(20)
global gripper_io
gripper_io = IODeviceInterface("end_effector", 'right_gripper')
s = rospy.Service('grasping', grasping, execute_grasp)
rospy.spin()
class Navigator(object):
"""
Interface class for a Navigator on the Intera Research Robot.
Signals:
button_square_changed - OFF/CLICK/LONG_PRESS/DOUBLE_CLICK
button_ok_changed
button_back_changed
button_show_changed
button_triangle_changed
button_circle_changed
wheel_changed - Wheel value
"""
def __init__(self):
"""
Constructor.
"""
self._navigator_io = IODeviceInterface("robot", "navigator")
self._button_lookup = {
0: 'OFF',
1: 'CLICK',
2: 'LONG_PRESS',
3: 'DOUBLE_CLICK'
}
def list_all_items(self):
"""
Returns a list of strings describing all available navigator items
@rtype: list
@return: a list of string representing navigator items
Each item name of the following format:
'<assembly>_button_<function>'
"""
return self._navigator_io.list_signal_names()
def get_wheel_state(self, wheel_name):
"""
Current state of the wheel providing wheel name
@type wheel_name: str
@param wheel_name: the wheel name
@rtype: uint
@return: an integer representing how far the wheel has turned
"""
return self._get_item_state(wheel_name)
def get_button_state(self, button_name):
"""
Current button state by providing button name
@type button_name: str
@param button_name: the button name
@rtype: uint
@return: an integer representing button values
Valid states:
{0:'OFF', 1:'CLICK', 2:'LONG_PRESS', 3:'DOUBLE_CLICK'}
"""
return self._get_item_state(button_name)
def button_string_lookup(self, button_value):
"""
Returns strings corresponding to the button state.
@type button_value: int
@param button_value: the value to lookup
@rtype: str
@return: 'INVALID_VALUE' if out of range, or if valid:
{0:'OFF', 1:'CLICK', 2:'LONG_PRESS', 3:'DOUBLE_CLICK'}
"""
if button_value in self._button_lookup:
return self._button_lookup[button_value]
else:
return 'INVALID_VALUE'
def register_callback(self, callback_function, signal_name, poll_rate=10):
"""
Registers a supplied callback to a change in state of supplied
signal_name's value. Spawns a thread that will call the callback with
the updated value.
@type callback_function: function
@param callback_function: function handle for callback function
@type signal_name: str
@param signal_name: the name of the signal to poll for value change
@type poll_rate: int
@param poll_rate: the rate at which to poll for a value change (in a separate
thread)
@rtype: str
@return: callback_id retuned if the callback was registered, and an
empty string if the requested signal_name does not exist in the
Navigator
"""
return self._navigator_io.register_callback(
callback_function=callback_function,
signal_name=signal_name,
poll_rate=poll_rate)
def deregister_callback(self, callback_id):
"""
Deregisters a callback based on the supplied callback_id.
@type callback_id: str
@param callback_id: the callback_id string to deregister
@rtype: bool
@return: returns bool True if the callback was successfully
deregistered, and False otherwise.
"""
return self._navigator_io.deregister_callback(callback_id)
def _get_item_state(self, item_name):
return self._navigator_io.get_signal_value(item_name)
class Gripper(object):
"""
Interface class for a gripper on the Intera Research Robot.
"""
MAX_POSITION = 0.041667
MIN_POSITION = 0.0
MAX_FORCE = 10.0
MIN_FORCE = 0.0
MAX_VELOCITY = 3.0
MIN_VELOCITY = 0.15
def __init__(self, side="right", calibrate=True):
"""
Constructor.
@type side: str
@param side: robot gripper name
@type calibrate: bool
@param calibrate: Attempts to calibrate the gripper when initializing class (defaults True)
"""
self.devices = None
self.name = '_'.join([side, 'gripper'])
self.ee_config_sub = rospy.Subscriber('/io/end_effector/config',
IONodeConfiguration,
self._config_callback)
# Wait for the gripper device status to be true
intera_dataflow.wait_for(
lambda: not self.devices is None,
timeout=5.0,
timeout_msg=(
"Failed to get gripper. No gripper attached on the robot."))
self.gripper_io = IODeviceInterface("end_effector", self.name)
if self.has_error():
self.reboot()
calibrate = True
if calibrate and self.is_calibrated():
self.calibrate()
def _config_callback(self, msg):
"""
config topic callback
"""
if msg.devices != []:
if str(msg.devices[0].name) == self.name:
self.devices = msg
def reboot(self):
"""
Power cycle the gripper, removing calibration information.
Basic call to the gripper reboot command. Waits for gripper to return
ready state but does not clear errors that could occur during boot.
@rtype: bool
@return: True if successfully Rebooted, False otherwise
"""
return self.gripper_io.set_signal_value("reboot", True)
def stop(self):
"""
Set the gripper to stop executing command at the current
position, apply holding force.
@rtype: bool
@return: True if successfully Stopped, False otherwise
"""
return self.gripper_io.set_signal_value("go", False)
def start(self):
"""
Set the gripper to start executing command at the current
position, apply holding force.
@rtype: bool
@return: True if successfully Started, False otherwise
"""
return self.gripper_io.set_signal_value("go", True)
def open(self, position=MAX_POSITION):
"""
Set the gripper position to open by providing opening position.
@type: float
@param: the postion of gripper in meters
@rtype: bool
@return: True if successfully set Position, False otherwise
"""
return self.gripper_io.set_signal_value("position_m", position)
def close(self, position=MIN_POSITION):
"""
Set the gripper position to close by providing closing position.
@type: float
@param: the postion of gripper in meters
@rtype: bool
@return: True if successfully set Position, False otherwise
"""
return self.gripper_io.set_signal_value("position_m", position)
def has_error(self):
"""
Returns a bool describing whether the gripper is in an error state.
(True: Error detected, False: No errors)
@rtype: bool
@return: True if in error state, False otherwise
"""
return self.gripper_io.get_signal_value("has_error")
def is_ready(self):
"""
Returns bool describing if the gripper ready, i.e. is
calibrated, not busy (as in resetting or rebooting), and
not moving.
@rtype: bool
@return: True if in ready state, False otherwise
"""
return (self.is_calibrated() and not self.has_error()
and not self.is_moving())
def is_moving(self):
"""
Returns bool describing if the gripper is moving.
@rtype: bool
@return: True if in moving state, False otherwise
"""
return self.gripper_io.get_signal_value("is_moving")
def is_gripping(self):
"""
Returns bool describing if the gripper is gripping.
@rtype: bool
@return: True if in gripping state, False otherwise
"""
return self.gripper_io.get_signal_value("is_gripping")
def is_calibrated(self):
"""
Returns bool describing if the gripper is calibrated.
@rtype: bool
@return: True if successfully calibrated, False otherwise
"""
return self.gripper_io.get_signal_value("is_calibrated")
def calibrate(self):
"""
Calibrate the gripper in order to set maximum and
minimum travel distance.
@rtype: bool
@return: True if successfully calibrating, False otherwise
"""
return self.gripper_io.set_signal_value("calibrate", True)
def get_position(self):
"""
Returns float describing the gripper position in meters.
@rtype: float
@return: Current Position value in Meters (m)
"""
return self.gripper_io.get_signal_value("position_response_m")
def set_position(self, position):
"""
Set the position of gripper.
@type: float
@param: the postion of gripper in meters
@rtype: bool
@return: True if successfully set value, False otherwise
"""
return self.gripper_io.set_signal_value("position_m", position)
def set_velocity(self, speed):
"""
Set the velocity at which the gripper position movement will execute.
@type: float
@param: the velocity of gripper in meters per second
@rtype: float
@return: Current Velocity value in Meters / second (m/s)
"""
return self.gripper_io.set_signal_value("speed_mps", speed)
def get_force(self):
"""
Returns the force sensed by the gripper in estimated Newtons.
@rtype: float
@return: Current Force value in Newton-Meters (N-m)
"""
return self.gripper_io.get_signal_value("force_response_n")
def set_holding_force(self, holding_force):
"""
Set holding force of successful gripper grasp.
Set the force at which the gripper will continue applying after a
position command has completed either from successfully achieving the
commanded position, or by exceeding the moving force threshold.
@type: float
@param: the holding force of gripper in newton
@rtype: bool
@return: True if successfully set value, False otherwise
"""
return self.gripper_io.set_signal_value("holding_force_n",
holding_force)
def set_object_weight(self, object_weight):
"""
Set the weight of the object in kilograms.
@type: float
@param: the object weight in newton
@rtype: bool
@return: True if successfully set value, False otherwise
"""
return self.gripper_io.set_signal_value("object_kg", object_weight)
def set_dead_zone(self, dead_zone):
"""
Set the gripper dead zone describing the position error threshold
where a move will be considered successful.
@type: float
@param: the dead zone of gripper in meters
@rtype: bool
@return: True if successfully set value, False otherwise
"""
return self.gripper_io.set_signal_value("dead_zone_m", dead_zone)
class beerGrabber():
def __init__(self):
# let's create MoveIt! objects:
self.scene = moveit_commander.PlanningSceneInterface()
self.robot = moveit_commander.RobotCommander()
self.group = moveit_commander.MoveGroupCommander("right_arm")
self.right_arm = ['right_j0', 'right_j1', 'right_j2', 'right_j3',
'right_j4', 'right_j5', 'right_j6']
rospy.loginfo("Initializing beerGrabber")
# ik service
self.ik_service_name = DEFAULT_IK_SERVICE
self.ik_serv = rospy.ServiceProxy(self.ik_service_name, SolvePositionIK)
self.ik_serv.wait_for_service()
rospy.loginfo("Successful connection to '" + self.ik_service_name + "'.")
# vision service
self.vision_service_name = DEFAULT_VISION_SERVICE
self.vision_serv = rospy.ServiceProxy(self.vision_service_name, target)
self.vision_serv.wait_for_service()
rospy.loginfo("Successful connection to '" + self.ik_service_name + "'.")
# check state service
self.check_service_name = DEFAULT_CHECK_SERVICE
self.check_serv = rospy.ServiceProxy(self.check_service_name, GetStateValidity)
self.check_serv.wait_for_service()
rospy.loginfo("Successful connection to '" + self.check_service_name + "'.")
# initiate gripper
side="right"
grip_name = '_'.join([side, 'gripper'])
self.gripper_io = IODeviceInterface("end_effector", grip_name)
# add target position - might be useful when add bottle in the scene
self.target = PoseStamped()
self.dict = collisionObjects()
def getTargetEEF(self):
"""
Get the target position in Cartesian Space from vision node
@return type: returns Pose() msg
"""
req = targetRequest()
req.data = 0
resp = self.vision_serv.call(req)
return resp.pose
def convertToJointStates(self, target):
"""
Using IK solver to convert the target position to Joint Space from Cartesian Space
@param target: Pose() msg to be converted
@return type: returns a list of joint values
"""
ikreq = SolvePositionIKRequest()
p = PoseStamped()
p.header = Header(stamp=rospy.Time.now(), frame_id='base')
p.pose = target
print "========= target 0 ========="
print target
# add target position - might be useful when add bottle in the scene
self.target = p
# Add desired pose for inverse kinematics
ikreq.pose_stamp.append(p)
# Request inverse kinematics from base to "right_hand" link
ikreq.tip_names.append('right_hand')
# The joint seed is where the IK position solver starts its optimization
ikreq.seed_mode = ikreq.SEED_USER
seed = JointState()
seed.name = self.right_arm
# use random numbers to get different solutions
# TODO: how to define the limitation
j1 = random.randint(50,340)/100.0
j2 = random.randint(40,300)/100.0
j3 = random.randint(-180,140)/100.0
j4 = random.randint(-150,10)/100.0
j5 = random.randint(-180,10)/100.0
j6 = random.randint(-200,10)/100.0
j7 = random.randint(-100,200)/100.0
# j1 range 0.5 to 3.4
seed.position = [j1, j2, j3, j4, j5, j6, j7]
# seed.position = [j1, 0.4, -1.7, 1.4, -1.1, -1.6, -0.4]
ikreq.seed_angles.append(seed)
# get the response from IK solver Service
resp = self.ik_serv.call(ikreq)
print "========= resp 0 ========="
print resp
# reassgin a random value to joint seed if fail to get the valid solution
while resp.result_type[0] <= 0:
print "error type: "
print resp.result_type[0]
j1 = random.randint(50,340)/100.0
seed.position = [j1, 0.4, -1.7, 1.4, -1.1, -1.6, -0.4]
ikreq.seed_angles.append(seed)
resp = self.ik_serv.call(ikreq)
# message print out
rospy.loginfo("SUCCESS - Valid Joint Solution Found")
# rospy.logdebug("Response message: " + str(resp))
target_js = [resp.joints[0].position[0],
resp.joints[0].position[1],
resp.joints[0].position[2],
resp.joints[0].position[3],
resp.joints[0].position[4],
resp.joints[0].position[5],
resp.joints[0].position[6]]
# rospy.logdebug("Target value: " + str(target_js))
return target_js
def addCollisionObjects(self,name=[""],p=PoseStamped(),p_size=size):
# TODO: add color
rospy.sleep(5)
for n in name:
# clear world
self.scene.remove_world_object(name)
# read pose() and size data
if name in self.dict.obj_pose_dict:
p = self.dict.obj_pose_dict[name]
if name in self.dict.obj_size_dict:
p_size = self.dict.obj_size_dict[name]
# set header.frame_id
p.header.frame_id = self.robot.get_planning_frame()
# add to scene
self.scene.add_box(name, p, p_size)
rospy.loginfo("Add: " + str(name) + " !")
return
def checkCollisions(self,target):
"""
Given a target position in JointState and check whether there is collision
@param target: a list of joints value to be checked
@return type: returns True when it is valid
"""
req = GetStateValidityRequest()
req.robot_state.joint_state.name = self.group.get_active_joints()
req.robot_state.joint_state.position = target
resp = self.check_serv(req)
print "Is target valid?", resp.valid
return resp.valid
def generateValidTargetJointState(self,pose):
"""Generate a valid target position in Joint Space and returns a list"""
target = self.convertToJointStates(pose)
while not self.checkCollisions(target):
target = self.convertToJointStates(pose)
return target
def testPlan(self,target):
print "test PLan:"
# set target
self.group.set_joint_value_target(target)
# generate plan
plan = self.group.plan()
# execute plan
self.group.go()
print "Done!"
def gripAct(self):
## Gripper
self.gripper_io.set_signal_value("position_m", 0.041)
rospy.sleep(2)
# side="right"
# grip_name = '_'.join([side, 'gripper'])
# gripper_io = IODeviceInterface("end_effector", grip_name)
if self.gripper_io.get_signal_value("is_calibrated") != True:
self.gripper_io.set_signal_value("calibrate", True)
## grabbing bottle
self.gripper_io.set_signal_value("speed_mps", 1)
self.gripper_io.set_signal_value("position_m", 0.01)
light_size = self.gripper_io.get_signal_value("position_response_m")
print "realeasing size is: ", light_size
rospy.sleep(1)
## if object is detected
if self.gripper_io.get_signal_value("is_gripping") != True:
print "Not stable!"
light_force = self.gripper_io.get_signal_value("force_response_n")
print "risky force is: ", light_force
self.gripper_io.set_signal_value("position_m", 0.00)
# get true force and obejct size responses
force = self.gripper_io.get_signal_value("force_response_n")
obj_size = self.gripper_io.get_signal_value("position_response_m")
print "force is: ", force
print "object size is: ", obj_size
def gripRelease(self):
touch_size = self.gripper_io.get_signal_value("position_response_m")
if self.gripper_io.get_signal_value("is_gripping"):
print touch_size
print "Releasing bottle!"
self.gripper_io.set_signal_value("position_m", 0.017)
def __init__(self):
"""
Constructor.
"""
self._lights_io = IODeviceInterface("robot", "robot")
class beerGrabber():
def __init__(self):
# let's create MoveIt! objects:
self.scene = moveit_commander.PlanningSceneInterface()
self.robot = moveit_commander.RobotCommander()
self.group = moveit_commander.MoveGroupCommander("right_arm")
self.group.set_max_velocity_scaling_factor(0.8)
self.group.set_max_acceleration_scaling_factor(0.8)
self.group.set_planning_time(60)
self.right_arm = ['right_j0', 'right_j1', 'right_j2', 'right_j3',
'right_j4', 'right_j5', 'right_j6']
rospy.loginfo("Initializing beerGrabber")
# ik service
self.ik_service_name = DEFAULT_IK_SERVICE
self.ik_serv = rospy.ServiceProxy(self.ik_service_name, SolvePositionIK)
self.ik_serv.wait_for_service()
rospy.loginfo("Successful connection to '" + self.ik_service_name + "'.")
# vision service
self.vision_service_name = DEFAULT_VISION_SERVICE
self.vision_serv = rospy.ServiceProxy(self.vision_service_name, target)
self.vision_serv.wait_for_service()
rospy.loginfo("Successful connection to '" + self.vision_service_name + "'.")
# check state service
self.check_service_name = DEFAULT_CHECK_SERVICE
# self.check_serv = rospy.ServiceProxy(self.check_service_name, GetStateValidity)
self.check_serv = rospy.ServiceProxy(self.check_service_name, GetStateValidity)
self.check_serv.wait_for_service()
rospy.loginfo("Successful connection to '" + self.check_service_name + "'.")
# initiate gripper
side="right"
grip_name = '_'.join([side, 'gripper'])
self.gripper_io = IODeviceInterface("end_effector", grip_name)
# add target position - might be useful when add bottle in the scene
self.target = PoseStamped()
self.dict = collisionObjects()
def getTargetEEF(self):
"""
Get the target position in Cartesian Space from vision node
@return type: returns Pose() msg
"""
req = targetRequest()
req.data = 0
resp = self.vision_serv.call(req)
return resp.pose
def convertToJointStates(self, target):
"""
Using IK solver to convert the target position to Joint Space from Cartesian Space
@param target: Pose() msg to be converted
@return type: returns a list of joint values
"""
ikreq = SolvePositionIKRequest()
p = PoseStamped()
p.header = Header(stamp=rospy.Time.now(), frame_id='base')
p.pose = target
print "========= target 0 ========="
print target
self.target.position.x = p.pose.position.x
self.target.position.y = p.pose.position.y
self.target.position.z = p.pose.position.z
# Add desired pose for inverse kinematics
ikreq.pose_stamp.append(p)
# Request inverse kinematics from base to "right_hand" link
ikreq.tip_names.append('right_hand')
# The joint seed is where the IK position solver starts its optimization
ikreq.seed_mode = ikreq.SEED_USER
seed = JointState()
seed.name = self.right_arm
# use random numbers to get different solutions
# j1 = random.randint(50,340)/100.0
# j2 = random.randint(40,300)/100.0
# j3 = random.randint(-180,140)/100.0
# j4 = random.randint(-150,10)/100.0
# j5 = random.randint(-180,10)/100.0
# j6 = random.randint(-200,10)/100.0
# j7 = random.randint(-100,200)/100.0
# seed.position = [j1, j2, j3, j4, j5, j6, j7]
seed.position = self.group.get_random_joint_values()
ikreq.seed_angles.append(seed)
# get the response from IK solver Service
resp = self.ik_serv.call(ikreq)
# reassgin a random value to joint seed if fail to get the valid solution
while resp.result_type[0] <= 0:
rospy.loginfo("error type: " + str(resp.result_type[0]))
seed.position = self.group.get_random_joint_values()
ikreq.seed_angles.append(seed)
resp = self.ik_serv.call(ikreq)
# message print out
rospy.loginfo("SUCCESS - Valid Joint Solution Found")
rospy.loginfo("Solution is:\n%s", resp.joints[0].position)
# rospy.logdebug("Response message: " + str(resp))
target_js = [resp.joints[0].position[0],
resp.joints[0].position[1],
resp.joints[0].position[2],
resp.joints[0].position[3],
resp.joints[0].position[4],
resp.joints[0].position[5],
resp.joints[0].position[6]]
return target_js
# def convertToJointStates(self, target):
# """Use ik solver of MoveIt"""
# rospy.loginfo("Attempting to solve IK")
# req = GetPositionIKRequest()
# req.ik_request.group_name = "right_arm"
# req.ik_request.robot_state.joint_state.name = self.group.get_active_joints()
# req.ik_request.robot_state.joint_state.position = [0, 0, 0, 0, 0, 0, 0]
# req.ik_request.avoid_collisions = True
# req.ik_request.timeout = rospy.Duration(3.0)
# req.ik_request.pose_stamped.header = Header(stamp=rospy.Time.now(), frame_id='base')
# req.ik_request.pose_stamped.pose = target
# req.ik_request.attempts = 10
# resp = self.ik_serv(req)
# # print "Error = ", resp.error_code
# # print "Solution = ", resp.solution.joint_state.position,"\r"
# q = np.zeros(len(self.group.get_active_joints()))
#
# while not resp.error_code.val == moveit_msgs.msg.MoveItErrorCodes.SUCCESS:
# req.ik_request.robot_state.joint_state.position = self.group.get_random_joint_values()
# resp = self.ik_serv(req)
#
# sol = {}
# for k,v in zip(resp.solution.joint_state.name, resp.solution.joint_state.position):
# sol[k] = v
# for i,n in enumerate(self.group.get_active_joints()):
# q[i] = sol[n]
# rospy.loginfo("Found IK solution! q = %s", str(q.tolist()))
#
# return q
def addCollisionObjects(self):
"""Add collision objects in the world scene"""
# TODO: add color
rospy.sleep(5)
# table scene
self.scene.remove_world_object("table1")
self.scene.remove_world_object("table2")
self.scene.remove_world_object("shelf")
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.pose.position.x = -0.5
p.pose.position.y = -1.0
p.pose.position.z = -0.211
self.scene.add_box("table1", p, (1.6,1.4,0.03))
p.pose.position.x = 1.0
p.pose.position.y = -0.5
p.pose.position.z = -0.211
self.scene.add_box("table2", p, (1.4,2.0,0.03))
p.pose.position.x = 0.55 + 0.38/2
p.pose.position.y = 0.7 + 0.78/2
p.pose.position.z = 1.47/2 - (1.47 - 0.92)
self.scene.add_box("shelf", p, (0.38, 0.78, 1.47))
print "Add tables and shelf!"
return
def checkCollisions(self,target):
"""
Given a target position in JointState and check whether there is collision
@param target: a list of joints value to be checked
@return type: returns True when it is valid
"""
req = GetStateValidityRequest()
req.robot_state.joint_state.name = self.group.get_active_joints()
req.robot_state.joint_state.position = target
resp = self.check_serv(req)
rospy.loginfo("Is target valid? --- " + str(resp.valid))
print "===================================="
return resp.valid
def generateValidTargetJointState(self,pose):
"""Generate a valid target position in Joint Space and returns a list"""
target = self.convertToJointStates(pose)
while not self.checkCollisions(target):
target = self.convertToJointStates(pose)
return target
def testPlan(self,target):
"""Set the target value, use MoveIt to generate the plan and execute it"""
print "Test PLan!"
# set target
self.group.set_joint_value_target(target)
# generate plan
plan = self.group.plan()
# execute plan
self.group.execute(plan)
print "Plan done!"
def gripAct(self, pose_target):
""" start to grab the bottle after moving into the target position """
# make sure the gripper is calibrated
if self.gripper_io.get_signal_value("is_calibrated") != True:
self.gripper_io.set_signal_value("calibrate", True)
# Make the gripper pully wide open, getting ready to grab
self.gripper_io.set_signal_value("position_m", 0.041)
rospy.sleep(1)
# move gripper closer using a sequence of waypoints
waypoints = []
wpose = pose_target
waypoints.append(copy.deepcopy(wpose))
for i in range(0, 3):
wpose.position.x += 0.05
waypoints.append(copy.deepcopy(wpose))
print waypoints
(grip_plan, fraction) = self.group.compute_cartesian_path(
waypoints, ## waypoints to follow
0.01, ## eef_step
0.0) ## jump_threshold
print fraction
self.group.execute(grip_plan)
rospy.sleep(1)
# grabbing bottle to a fairly tight position, and check how much force it's sensing right now
self.gripper_io.set_signal_value("speed_mps", 1)
self.gripper_io.set_signal_value("position_m", 0.00)
light_size = self.gripper_io.get_signal_value("position_response_m")
print "Loose size is: ", light_size
rospy.sleep(1)
# if the gripping is not tight enough, move the fingers closer to apply larger force
if self.gripper_io.get_signal_value("is_gripping") != True:
print "Not stable!"
light_force = self.gripper_io.get_signal_value("force_response_n")
print "Risky force is: ", light_force ## tell how much the force is that will be risky
self.gripper_io.set_signal_value("position_m", -0.03) ## move fingers closer
# get force and obejct size responses at the very tight status
force = self.gripper_io.get_signal_value("force_response_n")
obj_size = self.gripper_io.get_signal_value("position_response_m")
print "force is: ", force
print "object size is: ", obj_size
def gripRelease(self):
""" gripper releases the bottle after move into the final delivery location """
# comparing the force difference between right after grapping,
# and after moving into delivery location
final_force = self.gripper_io.get_signal_value("force_response_n")
print "final force is: ", final_force
# if the bottle is successfully moved into delivery location,
# then open the fingers to a position that has only a little grip,
# thus eaiser for human user to take it
if self.gripper_io.get_signal_value("is_gripping"):
print "Releasing bottle!"
self.gripper_io.set_signal_value("position_m", 0.012)
def add_grippers(self, zoffset=0.065, yoffset=0.048):
"""Attach the gripper to the Sawyer model"""
self.scene.remove_attached_object("right_gripper")
self.scene.remove_world_object("right_finger")
self.scene.remove_world_object("left_finger")
p = PoseStamped()
p.header.frame_id = "right_gripper"
p.pose.position.x = 0.0
p.pose.position.y = -yoffset
p.pose.position.z = zoffset
self.scene.attach_box("right_gripper", "right_finger", pose=p, size=(0.01, 0.01, 0.1))
p2 = PoseStamped()
p2.header.frame_id = "right_gripper"
p2.pose.position.x = 0.0
p2.pose.position.y = yoffset
p2.pose.position.z = zoffset
self.scene.attach_box("right_gripper", "left_finger", pose=p2, size=(0.01, 0.01, 0.1))
return
def add_bottle(self,target):
"""Add the bottle as collision object in the world scene"""
rospy.sleep(5)
# table scene
self.scene.remove_world_object("coke")
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.pose.position.x = target.position.x + 0.04
p.pose.position.y = target.position.y
p.pose.position.z = target.position.z + 0.12
self.scene.add_box("coke", p, (0.05,0.05,0.2))
print "Add coke!"
class SimpleClickSmartGripper(object):
"""
Bare bones Interface class for a ClickSmart gripper on the Intera Research Robot.
This ClickSmart interface uses "End Effector (EE) Signal Types" to
reference the Input/Output Signals configured on the ClickSmart device.
EE Signal Types include types such as "grip", (is)"open", (is)"closed",
(set)"object_kg". Each set of EE Signals is associated with a given
endpoint_id, but this endpoint_id can be left out in most of the interfaces
if a ClickSmart device only has a single endpoint configured.
ClickSmart devices can be configured using the Intera Studio GUI. The
configurations are stored on the ClickSmart device itself.
"""
def __init__(self, ee_device_id, initialize=True):
self.name = ee_device_id
self.endpoint_map = None
self.gripper_io = None
self._node_state = None
self._node_device_status = None
self._node_command_pub = rospy.Publisher('io/end_effector/command',
IOComponentCommand,
queue_size=10)
self._node_state_sub = rospy.Subscriber('io/end_effector/state',
IONodeStatus,
self._node_state_cb)
# Wait for the gripper device status to be true
intera_dataflow.wait_for(
lambda: self._node_device_status is not None,
timeout=5.0,
raise_on_error=initialize,
timeout_msg=(
"Failed to get gripper. No gripper attached on the robot."))
self.gripper_io = IODeviceInterface("end_effector", self.name)
self.gripper_io.config_changed.connect(self._load_endpoint_info)
if self.gripper_io.is_config_valid():
self._load_endpoint_info()
if initialize and self.needs_init():
self.initialize()
def _node_state_cb(self, msg):
# if we don't have a message yet, or if the timestamp is different: search for our device
if not self._node_state or IODeviceInterface.time_changed(
self._node_state.time, msg.time):
self._node_state = msg
if len(msg.devices) and msg.devices[0].name == self.name:
self._node_device_status = msg.devices[0].status
else:
# device removed
self._node_device_status = None
def _load_endpoint_info(self):
self._device_config = json.loads(self.config.device.config)
self.endpoint_map = self._device_config['params']['endpoints']
def is_ready(self):
"""
Returns bool describing if the gripper is ready to control,
(i.e. initialized ('activated'), not busy, no errors).
@rtype: bool
@return: True if in ready state, False otherwise
"""
return (self._node_device_status
and self._node_device_status.tag == 'ready'
and self.gripper_io.is_valid())
def needs_init(self):
"""
Returns True if device is not initialized and can be initialized
now ('activated').
@rtype: bool
@return: True if not initialized and can be, False otherwise
"""
return (self._node_device_status
and (self._node_device_status.tag == 'down'
or self._node_device_status.tag == 'unready'))
def initialize(self, timeout=5.0):
"""
Activate ClickSmart - initializes device and signals and attaches
the EE's URDF model to the internal RobotModel. Compensation
of the EE's added mass may cause arm to move.
@type timeout: float
@param timeout: timeout in seconds
"""
rospy.loginfo("Activating ClickSmart...")
cmd = IOCommand('activate', {"devices": [self.name]})
msg = cmd.as_msg()
self._node_command_pub.publish(msg)
if timeout:
intera_dataflow.wait_for(
lambda: self.is_ready(),
timeout=timeout,
timeout_msg=("Failed to initialize gripper."))
def get_ee_signal_value(self, ee_signal_type, endpoint_id=None):
"""
Return current value of the given EE Signal on the ClickSmart.
EE Signals are identified by "type" (such as "grip", (is)"open", etc.)
and are configured per endpoint. If the ClickSmart has multiple endpoints,
optionally specify the endpoint_id with which the signal is associated.
@type ee_signal_type: str
@param ee_signal_type: EE Signal Type of the signal to return; one of
those listed in get_ee_signals().
@type endpoint_id: str
@param endpoint_id: endpoint_id associated with signal; (default: 1st endpoint found)
@rtype: bool|float
@return: value of signal
"""
(ept_id, endpoint_info) = self.get_endpoint_info(endpoint_id)
if ee_signal_type in endpoint_info:
return self.get_signal_value(endpoint_info[ee_signal_type])
def set_ee_signal_value(self,
ee_signal_type,
value,
endpoint_id=None,
timeout=5.0):
"""
Sets the value of the given EE Signal on the ClickSmart.
EE Signals are identified by "type" (such as "grip", (is)"open", etc.)
and are configured per endpoint. If the ClickSmart has multiple endpoints,
optionally specify the endpoint_id with which the signal is associated.
@type ee_signal_type: str
@param ee_signal_type: EE Signal Type of the signal to return; one of
those listed in get_ee_signals().
@type value: bool|float
@param value: new value to set the signal to
@type endpoint_id: str
@param endpoint_id: endpoint_id associated with signal; (default: 1st endpoint found)
@type timeout: float
@param timeout: timeout in seconds
"""
(ept_id, endpoint_info) = self.get_endpoint_info(endpoint_id)
if ee_signal_type in endpoint_info:
self.set_signal_value(endpoint_info[ee_signal_type], value)
def get_ee_signals(self, endpoint_id=None):
"""
Returns dict of EE Signals by EE Signal Type for given endpoint.
Use the EE Signal Types (the keys) to specify signals in
get_ee_signal_value() and set_ee_signal_value().
EE Signals are identified by "type" (such as "grip", (is)"open", etc.)
and are configured per endpoint. If the ClickSmart has multiple endpoints,
optionally specify the endpoint_id with which the signal is associated.
@type endpoint_id: str
@param endpoint_id: endpoint_id associated with signal; (default: 1st endpoint found)
@rtype: dict({str:str})
@return: dict of EE Signal Types to signal_name
"""
(ept_id, endpoint_info) = self.get_endpoint_info(endpoint_id)
exclude = ['endpoint_id', 'label', 'type', 'actuationTimeS']
return {k: v for k, v in endpoint_info.iteritems() if k not in exclude}
def get_all_ee_signals(self):
"""
Returns dict of EE Signals by EE Signal Type for all endpoints,
organized under endpoint_id.
Use the EE Signal Types (the keys) to specify signals in
get_ee_signal_value() and set_ee_signal_value().
EE Signals are identified by "type" (such as "grip", (is)"open", etc.)
and are configured per endpoint. If the ClickSmart has multiple endpoints,
optionally specify the endpoint_id with which the signal is associated.
@type endpoint_id: str
@param endpoint_id: endpoint_id associated with signal; (default: 1st endpoint found)
@rtype: dict({str:dict({str:str})})
@return: dicts for each endpoint_id of EE Signal Types to signal_name
"""
info = dict()
for ept in self.list_endpoint_names():
info[ept] = self.get_ee_signals(ept)
return info
def get_all_signals(self):
"""
Returns generic info for all IO Signals on device, in a flat map keyed by signal_name.
@rtype: dict
@return: generic data-type, value, and IO Data for all signals
"""
return self.gripper_io.signals
def list_endpoint_names(self):
"""
Returns list of endpoint_ids currently configured on this ClickSmart device.
@rtype: list [str]
@return: list of endpoint_ids in ClickSmart config
"""
if self.endpoint_map:
return self.endpoint_map.keys()
else:
return []
def get_endpoint_info(self, endpoint_id=None):
"""
Returns the endpoint_id and endpoint info for the default endpoint,
or for the given endpoint_id if specified.
Use to find the default endpoint_id by calling without args.
Or lookup the current configuration info for a specific endpoint,
including EE Signals, and Intera UI labels.
@type endpoint_id: str
@param endpoint_id: optional endpoint_id to lookup; default: looksup
and returns info for the default endpoint_id of this ClickSmart
@rtype: tuple [ str, dict]
@return: (endpoint_id, endpoint_info) - the default endpoint_id (or the one specified)
and signal/config info associated with endpoint
"""
if self.endpoint_map is None or len(self.endpoint_map.keys()) <= 0:
rospy.logerr('Cannot use endpoint signals without any endpoints!')
return
endpoint_id = self.endpoint_map.keys(
)[0] if endpoint_id is None else endpoint_id
return (endpoint_id, self.endpoint_map[endpoint_id])
def send_configuration(self, config, timeout=5.0):
"""
Send a new JSON EndEffector configuration to the device.
@type config: dict
@param config: new json config to save on device
@type timeout: float
@param timeout: timeout in seconds - currently will wait for a minimum of
3 seconds before returning (to allow for reconfiguration)
"""
rospy.loginfo("Reconfiguring ClickSmart...")
cmd = IOCommand('reconfigure', {
"devices": {
self.name: config
},
"write_config": True
})
msg = cmd.as_msg()
self._node_command_pub.publish(msg)
if timeout:
# allow time for reconfiguration and storage write process
# TODO: use cmd acknowlegment timestamp, not hard-coded delay
delay = 3.0
delay_time = rospy.Time.now() + rospy.Duration(delay)
intera_dataflow.wait_for(
lambda: (rospy.Time.now() > delay_time and
(self.is_ready() or self.needs_init())),
timeout=max(timeout, delay),
body=lambda: self._node_command_pub.publish(msg),
timeout_msg=("Failed to reconfigure gripper."))
def __getattr__(self, name):
"""
This is a proxy-pass through mechanism that lets you look up methods and variables
on the underlying IODeviceInterface for the ClickSmart, as if they were on this class.
"""
return getattr(self.gripper_io, name)
class Gripper(object):
"""
Interface class for a gripper on the Intera Research Robot.
"""
MAX_POSITION = 0.041667
MIN_POSITION = 0.0
MAX_VELOCITY = 3.0
MIN_VELOCITY = 0.15
def __init__(self, ee_name="right_gripper", calibrate=True):
"""
Constructor.
@type name: str
@param name: robot gripper name (default: "right_gripper")
@type calibrate: bool
@param calibrate: Attempts to calibrate the gripper when initializing class (defaults True)
"""
self.devices = None
self.name = ee_name
if ee_name == 'right' or ee_name == 'left':
rospy.logwarn((
"Specifying gripper by 'side' is deprecated, please use full name"
" (ex: 'right_gripper')."))
self.name = '_'.join([ee_name, 'gripper'])
self.ee_config_sub = rospy.Subscriber('/io/end_effector/config',
IONodeConfiguration,
self._config_callback)
# Wait for the gripper device status to be true
intera_dataflow.wait_for(
lambda: not self.devices is None,
timeout=5.0,
timeout_msg=(
"Failed to get gripper. No gripper attached on the robot."))
self.gripper_io = IODeviceInterface("end_effector", self.name)
if self.has_error():
self.reboot()
calibrate = True
if calibrate and not self.is_calibrated():
self.calibrate()
def _config_callback(self, msg):
"""
config topic callback
"""
if msg.devices != []:
if str(msg.devices[0].name) == self.name:
self.devices = msg
def reboot(self):
"""
Power cycle the gripper, removing calibration information.
"""
self.gripper_io.set_signal_value("reboot", True)
def stop(self):
"""
Set the gripper to stop executing command at the current
position, apply holding force.
"""
self.gripper_io.set_signal_value("go", False)
def start(self):
"""
Set the gripper to start executing command at the current
position, apply holding force.
"""
self.gripper_io.set_signal_value("go", True)
def open(self, position=MAX_POSITION):
"""
Set the gripper position to open by providing opening position.
@type: float
@param: the postion of gripper in meters
"""
self.gripper_io.set_signal_value("position_m", position)
def close(self, position=MIN_POSITION):
"""
Set the gripper position to close by providing closing position.
@type: float
@param: the postion of gripper in meters
"""
self.gripper_io.set_signal_value("position_m", position)
def has_error(self):
"""
Returns a bool describing whether the gripper is in an error state.
(True: Error detected, False: No errors)
@rtype: bool
@return: True if in error state, False otherwise
"""
return self.gripper_io.get_signal_value("has_error")
def is_ready(self):
"""
Returns bool describing if the gripper ready, i.e. is
calibrated, not busy (as in resetting or rebooting), and
not moving.
@rtype: bool
@return: True if in ready state, False otherwise
"""
return (self.is_calibrated() and not self.has_error()
and not self.is_moving())
def is_moving(self):
"""
Returns bool describing if the gripper is moving.
@rtype: bool
@return: True if in moving state, False otherwise
"""
return self.gripper_io.get_signal_value("is_moving")
def is_gripping(self):
"""
Returns bool describing if the gripper is gripping.
@rtype: bool
@return: True if in gripping state, False otherwise
"""
return self.gripper_io.get_signal_value("is_gripping")
def is_calibrated(self):
"""
Returns bool describing if the gripper is calibrated.
@rtype: bool
@return: True if successfully calibrated, False otherwise
"""
return self.gripper_io.get_signal_value("is_calibrated")
def calibrate(self):
"""
Calibrate the gripper in order to set maximum and
minimum travel distance.
@rtype: bool
@return: True if calibration was successful, False otherwise
"""
self.gripper_io.set_signal_value("calibrate", True)
success = intera_dataflow.wait_for(lambda: self.is_calibrated(),
timeout=5.0,
raise_on_error=False)
if not success:
rospy.logerr("({0}) calibration failed.".format(self.name))
return success
def get_position(self):
"""
Returns float describing the gripper position in meters.
@rtype: float
@return: Current Position value in Meters (m)
"""
return self.gripper_io.get_signal_value("position_response_m")
def set_position(self, position):
"""
Set the position of gripper.
@type: float
@param: the postion of gripper in meters (m)
"""
self.gripper_io.set_signal_value("position_m", position)
def set_velocity(self, speed):
"""
DEPRECATED: Use set_cmd_velocity(speed)
Set the velocity at which the gripper position movement will execute.
@type: float
@param: the velocity of gripper in meters per second (m/s)
"""
rospy.logwarn("The set_velocity function is DEPRECATED. Please "
"update your code to use set_cmd_velocity() instead")
self.set_cmd_velocity(speed)
def set_cmd_velocity(self, speed):
"""
Set the commanded velocity at which the gripper position
movement will execute.
@type: float
@param: the velocity of gripper in meters per second (m/s)
"""
self.gripper_io.set_signal_value("speed_mps", speed)
def get_cmd_velocity(self):
"""
Get the commanded velocity at which the gripper position
movement executes.
@rtype: float
@return: the velocity of gripper in meters per second (m/s)
"""
return self.gripper_io.get_signal_value("speed_mps")
def get_force(self):
"""
Returns the force sensed by the gripper in estimated Newtons.
@rtype: float
@return: Current Force value in Newton-Meters (N-m)
"""
return self.gripper_io.get_signal_value("force_response_n")
def set_object_weight(self, object_weight):
"""
Set the weight of the object in kilograms.
Object mass is set as a point mass at the location of the tool endpoint
link in the URDF (e.g. 'right_gripper_tip'). The robot's URDF and
internal robot model will be updated to compensate for the mass.
@type: float
@param: the object weight in kilograms (kg)
"""
self.gripper_io.set_signal_value(self.name + "_tip_object_kg",
object_weight)
def get_object_weight(self):
"""
Get the currently configured weight of the object in kilograms.
Object mass is set as a point mass at the location of the tool endpoint
link in the URDF (e.g. 'right_gripper_tip'). The robot's URDF and
internal robot model will be updated to compensate for the mass.
@rtype: float
@return: the object weight in kilograms (kg)
"""
return self.gripper_io.get_signal_value(self.name + "_tip_object_kg")
def set_dead_zone(self, dead_zone):
"""
Set the gripper dead zone describing the position error threshold
where a move will be considered successful.
@type: float
@param: the dead zone of gripper in meters
"""
self.gripper_io.set_signal_value("dead_zone_m", dead_zone)
def get_dead_zone(self):
"""
Get the gripper dead zone describing the position error threshold
where a move will be considered successful.
@rtype: float
@return: the dead zone of gripper in meters
"""
return self.gripper_io.get_signal_value("dead_zone_m")
def set_holding_force(self, holding_force):
"""
@deprecated: Function deprecated. Holding force is now a fixed value.
"""
rospy.logerr(
"Removed variable holding force to improve gripper performance.")
return False
class Cuff(object):
"""
Interface class for cuff on the Intera robots.
"""
def __init__(self, limb="right"):
"""
Constructor.
@type limb: str
@param limb: limb side to interface
"""
params = RobotParams()
limb_names = params.get_limb_names()
if limb not in limb_names:
rospy.logerr("Cannot detect Cuff's limb {0} on this robot."
" Valid limbs are {1}. Exiting Cuff.init().".format(
limb, limb_names))
return
self.limb = limb
self.device = None
self.name = "cuff"
self.cuff_config_sub = rospy.Subscriber('/io/robot/cuff/config',
IODeviceConfiguration,
self._config_callback)
# Wait for the cuff status to be true
intera_dataflow.wait_for(
lambda: not self.device is None,
timeout=5.0,
timeout_msg=("Failed find cuff on limb '{0}'.".format(limb)))
self._cuff_io = IODeviceInterface("robot", self.name)
def _config_callback(self, msg):
"""
config topic callback
"""
if msg.device != []:
if str(msg.device.name) == self.name:
self.device = msg.device
def lower_button(self):
"""
Returns a boolean describing whether the lower button on cuff is pressed.
@rtype: bool
@return: a variable representing button state: (True: pressed, False: unpressed)
"""
return bool(
self._cuff_io.get_signal_value('_'.join(
[self.limb, "button_lower"])))
def upper_button(self):
"""
Returns a boolean describing whether the upper button on cuff is pressed.
(True: pressed, False: unpressed)
@rtype: bool
@return: a variable representing button state: (True: pressed, False: unpressed)
"""
return bool(
self._cuff_io.get_signal_value('_'.join(
[self.limb, "button_upper"])))
def cuff_button(self):
"""
Returns a boolean describing whether the cuff button on cuff is pressed.
(True: pressed, False: unpressed)
@rtype: bool
@return: a variable representing cuff button state: (True: pressed, False: unpressed)
"""
return bool(
self._cuff_io.get_signal_value('_'.join([self.limb, "cuff"])))
def register_callback(self, callback_function, signal_name, poll_rate=10):
"""
Registers a supplied callback to a change in state of supplied
signal_name's value. Spawns a thread that will call the callback with
the updated value.
@type callback_function: function
@param callback_function: function handle for callback function
@type signal_name: str
@param signal_name: the name of the signal to poll for value change
@type poll_rate: int
@param poll_rate: the rate at which to poll for a value change (in a separate
thread)
@rtype: str
@return: callback_id retuned if the callback was registered, and an
empty string if the requested signal_name does not exist in the
Navigator
"""
return self._cuff_io.register_callback(
callback_function=callback_function,
signal_name=signal_name,
poll_rate=poll_rate)
def deregister_callback(self, callback_id):
"""
Deregisters a callback based on the supplied callback_id.
@type callback_id: str
@param callback_id: the callback_id string to deregister
@rtype: bool
@return: returns bool True if the callback was successfully
deregistered, and False otherwise.
"""
return self._cuff_io.deregister_callback(callback_id)
