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)
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)
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)
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 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_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
