def __init__(self, name=None, prefix=None, hand_parameters=None, hand_serial=None, hand_number=0):
"""
Initialize the hand commander, using either a name + prefix, or the parameters returned by the hand finder.
@param name - name of the MoveIt group
@param prefix - prefix used for the tactiles and max_force
@param hand_parameters - from hand_finder.get_hand_parameters(). Will overwrite the name and prefix
@param hand_serial - which hand are you using
@param hand_number - which hand in a multi-hand system to use (starting at 0 and sorted by name/serial).
"""
self._hand_h = False
if name is None and prefix is None and hand_parameters is None and hand_serial is None:
hand_finder = HandFinder()
if hand_finder.hand_e_available():
name, prefix, hand_serial = hand_finder.get_hand_e(number=hand_number)
elif hand_finder.hand_h_available():
self._hand_h = True
name, prefix, hand_serial = hand_finder.get_hand_h(number=hand_number)
else:
rospy.logfatal("No hands found and no information given to define hand commander.")
raise SrRobotCommanderException("No hand found.")
elif hand_parameters is not None:
# extracting the name and prefix from the hand finder parameters
if len(hand_parameters.mapping) == 0:
rospy.logfatal("No hand detected")
raise SrRobotCommanderException("No hand found.")
hand_mapping = hand_parameters.mapping[hand_serial]
prefix = hand_parameters.joint_prefix[hand_serial]
if name is None:
if hand_mapping == 'rh':
name = "right_hand"
else:
name = "left_hand"
else:
if name is None:
name = "right_hand"
if prefix is None:
prefix = "rh_"
super(SrHandCommander, self).__init__(name)
if not self._hand_h:
self._tactiles = TactileReceiver(prefix)
# appends trailing slash if necessary
self._topic_prefix = prefix
if self._topic_prefix and self._topic_prefix.endswith("_"):
self._topic_prefix = self._topic_prefix[:-1] # Remove trailing _
if self._topic_prefix and not self._topic_prefix.endswith("/"):
self._topic_prefix += "/"
self._hand_serial = hand_serial
def __init__(self, hand_commander, demo_joint_states, prefix):
self.hand_commander = hand_commander
self.demo_joint_states = demo_joint_states
self.prefix = prefix
# Read tactile type
self.tactile_reciever = TactileReceiver(prefix)
self.tactile_type = self.tactile_reciever.get_tactile_type()
# Zero values in dictionary for tactile sensors (initialized at 0)
self.force_zero = {"FF": 0, "MF": 0, "RF": 0, "LF": 0, "TH": 0}
# Initialize values for current tactile values
self.tactile_values = {"FF": 0, "MF": 0, "RF": 0, "LF": 0, "TH": 0}
self.zero_tactile_sensors()
def __init__(self,
name=None,
prefix=None,
hand_parameters=None,
hand_serial=None):
"""
Initialize the hand commander, using either a name + prefix, or the parameters returned by the hand finder.
@param name - name of the MoveIt group
@param prefix - prefix used for the tactiles and max_force
@param hand_parameters - from hand_finder.get_hand_parameters(). Will overwrite the name and prefix
@param hand_serial - which hand are you using
"""
# extracting the name and prefix from the hand finder parameters
if hand_parameters is not None:
if len(hand_parameters.mapping) is 0:
rospy.logerr("No hand detected")
exit("no hand detected")
hand_mapping = hand_parameters.mapping[hand_serial]
prefix = hand_parameters.joint_prefix[hand_serial]
if name is None:
if hand_mapping == 'rh':
name = "right_hand"
else:
name = "left_hand"
else:
if name is None:
name = "right_hand"
if prefix is None:
prefix = "rh_"
super(SrHandCommander, self).__init__(name)
self._tactiles = TactileReceiver(prefix)
# appends trailing slash if necessary
self._topic_prefix = prefix
if self._topic_prefix and not self._topic_prefix.endswith("/"):
self._topic_prefix += "/"
self._hand_serial = hand_serial
def __init__(self):
"""
Builds the library, creates the communication node in ROS and
initializes the hand publisher and subscriber to the default
values of shadowhand_data and sendupdate
"""
self.allJoints = [Joint("THJ1", "smart_motor_th1", 0, 90),
Joint("THJ2", "smart_motor_th2", -40, 40),
Joint("THJ3", "smart_motor_th3", -15, 15),
Joint("THJ4", "smart_motor_th4", 0, 75),
Joint("THJ5", "smart_motor_th5", -60, 60),
Joint("FFJ0", "smart_motor_ff2", 0, 180),
Joint("FFJ3", "smart_motor_ff3"),
Joint("FFJ4", "smart_motor_ff4", -20, 20),
Joint("MFJ0", "smart_motor_mf2", 0, 180),
Joint("MFJ3", "smart_motor_mf3"),
Joint("MFJ4", "smart_motor_mf4", -20, 20),
Joint("RFJ0", "smart_motor_rf2", 0, 180),
Joint("RFJ3", "smart_motor_rf3"),
Joint("RFJ4", "smart_motor_rf4", -20, 20),
Joint("LFJ0", "smart_motor_lf2", 0, 180),
Joint("LFJ3", "smart_motor_lf3"),
Joint("LFJ4", "smart_motor_lf4", -20, 20),
Joint("LFJ5", "smart_motor_lf5", 0, 45),
Joint("WRJ1", "smart_motor_wr1", -45, 30),
Joint("WRJ2", "smart_motor_wr2", -30, 10),
]
self.handJoints = []
self.armJoints = [Joint("ShoulderJRotate", "", -45, 60),
Joint("ShoulderJSwing", "", 0, 80),
Joint("ElbowJSwing", "", 0, 120),
Joint("ElbowJRotate", "", -80, 80)
]
self.lastMsg = joints_data()
self.lastArmMsg = joints_data()
self.cyberglove_pub = 0
self.cyberglove_sub = 0
self.cybergrasp_pub = 0
self.cyberglove_sub = 0
self.isFirstMessage = True
self.isFirstMessageArm = True
self.isReady = False
self.liste = 0
self.hasarm = 0
self.dict_pos = {}
self.dict_tar = {}
self.dict_arm_pos = {}
self.dict_arm_tar = {}
self.dict_ethercat_joints = {}
self.eth_publishers = {}
self.eth_subscribers = {}
# rospy.init_node('python_hand_library')
self.sendupdate_lock = threading.Lock()
self.joint_states_lock = threading.Lock()
# contains the ending for the topic depending on which controllers are
# loaded
self.topic_ending = ""
# EtherCAT hand
self.activate_etherCAT_hand()
#
# Grasps
self.grasp_parser = GraspParser()
self.rootPath = rospkg.RosPack().get_path('sr_hand')
self.grasp_parser.parse_tree(
self.rootPath + "/scripts/sr_hand/grasps.xml")
self.grasp_interpoler = 0
self.pub = rospy.Publisher(
'srh/sendupdate', sendupdate, queue_size=1, latch=True)
self.pub_arm = rospy.Publisher(
'sr_arm/sendupdate', sendupdate, queue_size=1, latch=True)
self.sub_arm = rospy.Subscriber(
'sr_arm/shadowhand_data', joints_data, self.callback_arm)
self.sub = rospy.Subscriber(
'srh/shadowhand_data', joints_data, self.callback)
self.hand_type = self.check_hand_type()
self.hand_velocity = {}
self.hand_effort = {}
if (self.hand_type == "etherCAT") or (self.hand_type == "gazebo"):
self.joint_states_listener = rospy.Subscriber(
"joint_states", JointState, self.joint_states_callback)
# Initialize the command publishers here, to avoid the delay caused
# when initializing them in the sendupdate
for jnt in self.allJoints:
if jnt.name not in self.eth_publishers:
topic = "sh_" + \
jnt.name.lower() + self.topic_ending + "/command"
self.eth_publishers[jnt.name] = rospy.Publisher(
topic, Float64, queue_size=1, latch=True)
self.tactile_receiver = TactileReceiver()
threading.Thread(None, rospy.spin)
class ShadowHand_ROS(object):
"""
This is a python library used to easily access the shadow hand ROS interface.
"""
def __init__(self):
"""
Builds the library, creates the communication node in ROS and
initializes the hand publisher and subscriber to the default
values of shadowhand_data and sendupdate
"""
self.allJoints = [Joint("THJ1", "smart_motor_th1", 0, 90),
Joint("THJ2", "smart_motor_th2", -40, 40),
Joint("THJ3", "smart_motor_th3", -15, 15),
Joint("THJ4", "smart_motor_th4", 0, 75),
Joint("THJ5", "smart_motor_th5", -60, 60),
Joint("FFJ0", "smart_motor_ff2", 0, 180),
Joint("FFJ3", "smart_motor_ff3"),
Joint("FFJ4", "smart_motor_ff4", -20, 20),
Joint("MFJ0", "smart_motor_mf2", 0, 180),
Joint("MFJ3", "smart_motor_mf3"),
Joint("MFJ4", "smart_motor_mf4", -20, 20),
Joint("RFJ0", "smart_motor_rf2", 0, 180),
Joint("RFJ3", "smart_motor_rf3"),
Joint("RFJ4", "smart_motor_rf4", -20, 20),
Joint("LFJ0", "smart_motor_lf2", 0, 180),
Joint("LFJ3", "smart_motor_lf3"),
Joint("LFJ4", "smart_motor_lf4", -20, 20),
Joint("LFJ5", "smart_motor_lf5", 0, 45),
Joint("WRJ1", "smart_motor_wr1", -45, 30),
Joint("WRJ2", "smart_motor_wr2", -30, 10),
]
self.handJoints = []
self.armJoints = [Joint("ShoulderJRotate", "", -45, 60),
Joint("ShoulderJSwing", "", 0, 80),
Joint("ElbowJSwing", "", 0, 120),
Joint("ElbowJRotate", "", -80, 80)
]
self.lastMsg = joints_data()
self.lastArmMsg = joints_data()
self.cyberglove_pub = 0
self.cyberglove_sub = 0
self.cybergrasp_pub = 0
self.cyberglove_sub = 0
self.isFirstMessage = True
self.isFirstMessageArm = True
self.isReady = False
self.liste = 0
self.hasarm = 0
self.dict_pos = {}
self.dict_tar = {}
self.dict_arm_pos = {}
self.dict_arm_tar = {}
self.dict_ethercat_joints = {}
self.eth_publishers = {}
self.eth_subscribers = {}
# rospy.init_node('python_hand_library')
self.sendupdate_lock = threading.Lock()
self.joint_states_lock = threading.Lock()
# contains the ending for the topic depending on which controllers are
# loaded
self.topic_ending = ""
# EtherCAT hand
self.activate_etherCAT_hand()
#
# Grasps
self.grasp_parser = GraspParser()
self.rootPath = rospkg.RosPack().get_path('sr_hand')
self.grasp_parser.parse_tree(
self.rootPath + "/scripts/sr_hand/grasps.xml")
self.grasp_interpoler = 0
self.pub = rospy.Publisher(
'srh/sendupdate', sendupdate, queue_size=1, latch=True)
self.pub_arm = rospy.Publisher(
'sr_arm/sendupdate', sendupdate, queue_size=1, latch=True)
self.sub_arm = rospy.Subscriber(
'sr_arm/shadowhand_data', joints_data, self.callback_arm)
self.sub = rospy.Subscriber(
'srh/shadowhand_data', joints_data, self.callback)
self.hand_type = self.check_hand_type()
self.hand_velocity = {}
self.hand_effort = {}
if (self.hand_type == "etherCAT") or (self.hand_type == "gazebo"):
self.joint_states_listener = rospy.Subscriber(
"joint_states", JointState, self.joint_states_callback)
# Initialize the command publishers here, to avoid the delay caused
# when initializing them in the sendupdate
for jnt in self.allJoints:
if jnt.name not in self.eth_publishers:
topic = "sh_" + \
jnt.name.lower() + self.topic_ending + "/command"
self.eth_publishers[jnt.name] = rospy.Publisher(
topic, Float64, queue_size=1, latch=True)
self.tactile_receiver = TactileReceiver()
threading.Thread(None, rospy.spin)
def create_grasp_interpoler(self, current_step, next_step):
self.grasp_interpoler = GraspInterpoler(current_step, next_step)
def callback_ethercat_states(self, data, jointName):
"""
@param data: The ROS message received which called the callback
If the message is the first received, initializes the dictionnaries
Else, it updates the lastMsg
@param joint: a Joint object that contains the name of the joint that we receive data from
"""
joint_data = joint(joint_name=jointName, joint_target=math.degrees(
float(data.set_point)), joint_position=math.degrees(float(data.process_value)))
# update the dictionary of joints
self.dict_ethercat_joints[joint_data.joint_name] = joint_data
# Build a CAN hand style lastMsg with the latest info in
# self.dict_ethercat_joints
self.lastMsg = joints_data()
for joint_name in self.dict_ethercat_joints.keys():
self.lastMsg.joints_list.append(
self.dict_ethercat_joints[joint_name])
# self.lastMsg.joints_list_length++
# TODO This is not the right way to do it. We should check that messages from all the joints have been
# received (but we don't know if we have all the joints, e.g three
# finger hand)
self.isReady = True
# if self.isFirstMessage :
# self.init_actual_joints()
# for joint in self.lastMsg.joints_list :
# self.dict_pos[joint.joint_name]=joint.joint_position
# self.dict_tar[joint.joint_name]=joint.joint_target
# self.isFirstMessage = False
# self.isReady = True
def callback(self, data):
"""
@param data: The ROS message received which called the callback
If the message is the first received, initializes the dictionnaries
Else, it updates the lastMsg
"""
self.lastMsg = data
if self.isFirstMessage:
self.init_actual_joints()
for jnt in self.lastMsg.joints_list:
self.dict_pos[jnt.joint_name] = jnt.joint_position
self.dict_tar[jnt.joint_name] = jnt.joint_target
self.isFirstMessage = False
self.isReady = True
def callback_arm(self, data):
"""
@param data: The ROS message received which called the callback
If the message is the first received, initializes the dictionnaries
Else, it updates the lastMsg
"""
self.lastArmMsg = data
if self.isFirstMessageArm:
for jnt in self.lastArmMsg.joints_list:
self.dict_arm_pos[jnt.joint_name] = jnt.joint_position
self.dict_arm_tar[jnt.joint_name] = jnt.joint_target
def init_actual_joints(self):
"""
Initializes the library with just the fingers actually connected
"""
for joint_all in self.allJoints:
for joint_msg in self.lastMsg.joints_list:
if joint_msg.joint_name == joint_all.name:
self.handJoints.append(joint_all)
def check_hand_type(self):
"""
@return : true if some hand is detected
"""
if self.check_gazebo_hand_presence():
return "gazebo"
elif self.check_etherCAT_hand_presence():
return "etherCAT"
elif self.check_CAN_hand_presence():
return "CANhand"
return None
def check_CAN_hand_presence(self):
"""
@return : true if the CAN hand is detected
"""
t = 0.0
while not self.isReady:
time.sleep(1.0)
t = t + 1.0
rospy.loginfo(
"Waiting for service since " + str(t) + " seconds...")
if t >= 5.0:
rospy.logerr(
"No hand found. Are you sure the ROS hand is running ?")
return False
return True
def set_shadowhand_data_topic(self, topic):
"""
@param topic: The new topic to be set as the hand publishing topic
Set the library to listen to a new topic
"""
print 'Changing subscriber to ' + topic
self.sub = rospy.Subscriber(topic, joints_data, self.callback)
def set_sendupdate_topic(self, topic):
"""
@param topic: The new topic to be set as the hand subscribing topic
Set the library to publish to a new topic
"""
print 'Changing publisher to ' + topic
self.pub = rospy.Publisher(topic, sendupdate, latch=True)
def sendupdate_from_dict(self, dicti):
"""
@param dicti: Dictionnary containing all the targets to send, mapping
the name of the joint to the value of its target.
Sends new targets to the hand from a dictionnary
"""
self.sendupdate_lock.acquire()
if (self.hand_type == "etherCAT") or (self.hand_type == "gazebo"):
for join in dicti.keys():
if join not in self.eth_publishers:
topic = "sh_" + \
join.lower() + self.topic_ending + "/command"
self.eth_publishers[join] = rospy.Publisher(
topic, Float64, latch=True)
msg_to_send = Float64()
msg_to_send.data = math.radians(float(dicti[join]))
self.eth_publishers[join].publish(msg_to_send)
elif self.hand_type == "CANhand":
message = []
for join in dicti.keys():
message.append(
joint(joint_name=join, joint_target=dicti[join]))
self.pub.publish(sendupdate(len(message), message))
self.sendupdate_lock.release()
def sendupdate(self, jointName, angle=0):
"""
@param jointName: Name of the joint to update
@param angle: Target of the joint, 0 if not set
Sends a new target for the specified joint
"""
self.sendupdate_lock.acquire()
if (self.hand_type == "etherCAT") or (self.hand_type == "gazebo"):
if jointName not in self.eth_publishers:
topic = "sh_" + \
jointName.lower() + self.topic_ending + "/command"
self.eth_publishers[jointName] = rospy.Publisher(
topic, Float64, latch=True)
msg_to_send = Float64()
msg_to_send.data = math.radians(float(angle))
self.eth_publishers[jointName].publish(msg_to_send)
elif self.hand_type == "CANhand":
message = [joint(joint_name=jointName, joint_target=angle)]
self.pub.publish(sendupdate(len(message), message))
self.sendupdate_lock.release()
def sendupdate_arm_from_dict(self, dicti):
"""
@param dicti: Dictionnary containing all the targets to send, mapping
the name of the joint to the value of its target.
Sends new targets to the hand from a dictionnary
"""
message = []
for join in dicti.keys():
message.append(joint(joint_name=join, joint_target=dicti[join]))
self.pub_arm.publish(sendupdate(len(message), message))
def sendupdate_arm(self, jointName, angle=0):
"""
@param jointName: Name of the joint to update
@param angle: Target of the joint, 0 if not set
Sends a new target for the specified joint
"""
message = [joint(joint_name=jointName, joint_target=angle)]
self.pub_arm.publish(sendupdate(len(message), message))
def valueof(self, jointName):
"""
@param jointName: Name of the joint to read the value
@return: 'NaN' if the value is not correct, the actual position of the joint else
"""
for jnt in self.lastMsg.joints_list:
if jnt.joint_name == jointName:
return float(jnt.joint_position)
for jnt in self.lastArmMsg.joints_list:
if jnt.joint_name == jointName:
return float(jnt.joint_position)
return 'NaN'
def has_arm(self):
"""
@return : True if an arm is detected on the roscore
"""
if not self.hasarm == 0:
return self.hasarm
self.hasarm = False
if self.liste == 0:
master = rosgraph.masterapi.Master('/rostopic')
self.liste = master.getPublishedTopics('/')
for topic_typ in self.liste:
for topic in topic_typ:
if 'sr_arm/shadowhand_data' in topic:
self.hasarm = True
return self.hasarm
def record_step_to_file(self, filename, grasp_as_xml):
"""
@param filename: name (or path) of the file to save to
@param grasp_as_xml: xml-formatted grasp
Write the grasp at the end of the file, creates the file if does not exist
"""
if os.path.exists(filename):
obj = open(filename, 'r')
text = obj.readlines()
obj.close()
objWrite = open(filename, 'w')
for index in range(0, len(text) - 1):
objWrite.write(text[index])
objWrite.write(grasp_as_xml)
objWrite.write('</root>')
objWrite.close()
else:
objWrite = open(filename, 'w')
objWrite.write('<root>\n')
objWrite.write(grasp_as_xml)
objWrite.write('</root>')
objWrite.close()
def save_hand_position_to_file(self, filename):
objFile = open(filename, 'w')
for key, value in self.dict_pos:
objFile.write(key + ' ' + value + '\n')
objFile.close()
def read_all_current_positions(self):
"""
@return: dictionnary mapping joint names to actual positions
Read all the positions in the lastMsg
"""
if not self.isReady:
return
for jnt in self.lastMsg.joints_list:
self.dict_pos[jnt.joint_name] = jnt.joint_position
return self.dict_pos
def read_all_current_targets(self):
"""
@return: dictionnary mapping joint names to current targets
Read all the targets in the lastMsg
"""
for jnt in self.lastMsg.joints_list:
self.dict_tar[jnt.joint_name] = jnt.joint_target
return self.dict_tar
def read_all_current_velocities(self):
"""
@return: dictionary mapping joint names to current velocities
"""
with self.joint_states_lock:
return self.hand_velocity
def read_all_current_efforts(self):
"""
@return: dictionary mapping joint names to current efforts
"""
with self.joint_states_lock:
return self.hand_effort
def read_all_current_arm_positions(self):
"""
@return: dictionnary mapping joint names to actual positions
Read all the positions in the lastMsg
"""
for jnt in self.lastArmMsg.joints_list:
self.dict_arm_pos[jnt.joint_name] = jnt.joint_position
return self.dict_arm_pos
def read_all_current_arm_targets(self):
"""
@return: dictionnary mapping joint names to actual targets
Read all the targets in the lastMsg
"""
for jnt in self.lastArmMsg.joints_list:
self.dict_arm_tar[jnt.joint_name] = jnt.joint_target
return self.dict_arm_tar
def resend_targets(self):
"""
Resend the targets read in the lastMsg to the hand
"""
for key, value in self.dict_tar.items():
self.sendupdate(jointName=key, angle=value)
def callVisualisationService(self, callList=0, reset=0):
"""
@param callList: dictionnary mapping joint names to information that should be displayed
@param reset: flag used to tell if the parameters should be replaced by the new ones or
just added to the previous ones.
Calls a ROS service to display various information in Rviz
"""
if reset == 0:
print 'no reset'
if reset == 1:
print 'reset'
def check_etherCAT_hand_presence(self):
"""
Only used to check if a real etherCAT hand is detected in the system
check if something is being published to this topic, otherwise
return false
Bear in mind that the gazebo hand also publishes the joint_states topic,
so we need to check for the gazebo hand first
"""
try:
rospy.wait_for_message("joint_states", JointState, timeout=0.2)
except rospy.ROSException:
rospy.logwarn("no message received from joint_states")
return False
return True
def check_gazebo_hand_presence(self):
"""
Only used to check if a Gazebo simulated (etherCAT protocol) hand is detected in the system
check if something is being published to this topic, otherwise
return false
"""
try:
rospy.wait_for_message(
"gazebo/parameter_updates", Config, timeout=0.2)
except rospy.ROSException:
return False
return True
def activate_etherCAT_hand(self):
"""
At the moment we just try to use the mixed position velocity controllers
"""
success = True
for joint_all in self.allJoints:
self.topic_ending = "_position_controller"
topic = "sh_" + \
joint_all.name.lower() + self.topic_ending + "/state"
success = True
try:
rospy.wait_for_message(
topic, control_msgs.msg.JointControllerState, timeout=0.2)
except:
try:
self.topic_ending = "_mixed_position_velocity_controller"
topic = "sh_" + \
joint_all.name.lower() + self.topic_ending + "/state"
rospy.wait_for_message(
topic, JointControllerState, timeout=0.2)
except rospy.ROSException:
success = False
if success:
if self.topic_ending == "_mixed_position_velocity_controller":
self.eth_subscribers[joint_all.name] = rospy.Subscriber(
topic, JointControllerState, self.callback_ethercat_states, joint_all.name)
else:
self.eth_subscribers[joint_all.name] = rospy.Subscriber(
topic,
control_msgs.msg.JointControllerState,
self.callback_ethercat_states,
joint_all.name)
if len(self.eth_subscribers) > 0:
return True
return False
def joint_states_callback(self, joint_state):
"""
The callback function for the topic joint_states.
It will store the received joint velocity and effort information in two dictionaries
Velocity will be converted to degrees/s.
Effort units are kept as they are (currently ADC units, as no calibration is performed on the strain gauges)
@param joint_state: the message containing the joints data.
"""
with self.joint_states_lock:
self.hand_velocity = {n: math.degrees(v)
for n, v in zip(joint_state.name, joint_state.velocity)}
self.hand_effort = {
n: e for n, e in zip(joint_state.name, joint_state.effort)}
for finger in ['FF', 'MF', 'RF', 'LF']:
for dic in [self.hand_velocity, self.hand_effort]:
if (finger + 'J1') in dic and (finger + 'J2') in dic:
dic[finger + 'J0'] = dic[
finger + 'J1'] + dic[finger + 'J2']
del dic[finger + 'J1']
del dic[finger + 'J2']
def get_tactile_type(self):
return self.tactile_receiver.get_tactile_type()
def get_tactile_state(self):
return self.tactile_receiver.get_tactile_state()
class TactileReading():
def __init__(self, hand_commander, demo_joint_states, prefix):
self.hand_commander = hand_commander
self.demo_joint_states = demo_joint_states
self.prefix = prefix
# Read tactile type
self.tactile_reciever = TactileReceiver(prefix)
self.tactile_type = self.tactile_reciever.get_tactile_type()
# Zero values in dictionary for tactile sensors (initialized at 0)
self.force_zero = {"FF": 0, "MF": 0, "RF": 0, "LF": 0, "TH": 0}
# Initialize values for current tactile values
self.tactile_values = {"FF": 0, "MF": 0, "RF": 0, "LF": 0, "TH": 0}
self.zero_tactile_sensors()
def zero_tactile_sensors(self):
rospy.sleep(0.5)
rospy.logwarn(
'\nPLEASE ENSURE THAT THE TACTILE SENSORS ARE NOT PRESSED')
input('\nPress ENTER to continue...')
rospy.sleep(1.0)
for x in range(1, 1000):
# Read current state of tactile sensors to zero them
self.read_tactile_values()
for finger in ["FF", "MF", "RF", "LF", "TH"]:
if self.tactile_values[finger] > self.force_zero[finger]:
self.force_zero[finger] = self.tactile_values[finger] + 20
rospy.loginfo('Force Zero: ' + str(self.force_zero))
def read_tactile_values(self):
# Read current state of tactile sensors
tactile_state = self.tactile_reciever.get_tactile_state()
if self.tactile_type == "biotac":
self.tactile_values['FF'] = tactile_state.tactiles[0].pdc
self.tactile_values['MF'] = tactile_state.tactiles[1].pdc
self.tactile_values['RF'] = tactile_state.tactiles[2].pdc
self.tactile_values['LF'] = tactile_state.tactiles[3].pdc
self.tactile_values['TH'] = tactile_state.tactiles[4].pdc
elif self.tactile_type == "PST":
self.tactile_values['FF'] = tactile_state.pressure[0]
self.tactile_values['MF'] = tactile_state.pressure[1]
self.tactile_values['RF'] = tactile_state.pressure[2]
self.tactile_values['LF'] = tactile_state.pressure[3]
self.tactile_values['TH'] = tactile_state.pressure[4]
def get_tactiles(self):
if self.tactile_type is None:
rospy.loginfo(
"You don't have tactile sensors. " +
"Talk to your Shadow representative to purchase some " +
"or use the keyboard to access this demo.")
else:
# Zero tactile sensors
self.zero_tactile_sensors()
return self.tactile_type
def confirm_touched(self):
self.read_tactile_values()
touched = None
for finger in ["FF", "MF", "RF", "LF", "TH"]:
if self.tactile_values[finger] > self.force_zero[finger]:
touched = finger
rospy.loginfo("{} contact".format(touched))
return touched
class SrHandCommander(SrRobotCommander):
"""
Commander class for hand
"""
__set_force_srv = {}
def __init__(self, name=None, prefix=None, hand_parameters=None, hand_serial=None, hand_number=0):
"""
Initialize the hand commander, using either a name + prefix, or the parameters returned by the hand finder.
@param name - name of the MoveIt group
@param prefix - prefix used for the tactiles and max_force
@param hand_parameters - from hand_finder.get_hand_parameters(). Will overwrite the name and prefix
@param hand_serial - which hand are you using
@param hand_number - which hand in a multi-hand system to use (starting at 0 and sorted by name/serial).
"""
self._hand_h = False
if name is None and prefix is None and hand_parameters is None and hand_serial is None:
hand_finder = HandFinder()
if hand_finder.hand_e_available():
name, prefix, hand_serial = hand_finder.get_hand_e(number=hand_number)
elif hand_finder.hand_h_available():
self._hand_h = True
name, prefix, hand_serial = hand_finder.get_hand_h(number=hand_number)
else:
rospy.logfatal("No hands found and no information given to define hand commander.")
raise SrRobotCommanderException("No hand found.")
elif hand_parameters is not None:
# extracting the name and prefix from the hand finder parameters
if len(hand_parameters.mapping) == 0:
rospy.logfatal("No hand detected")
raise SrRobotCommanderException("No hand found.")
hand_mapping = hand_parameters.mapping[hand_serial]
prefix = hand_parameters.joint_prefix[hand_serial]
if name is None:
if hand_mapping == 'rh':
name = "right_hand"
else:
name = "left_hand"
else:
if name is None:
name = "right_hand"
if prefix is None:
prefix = "rh_"
super(SrHandCommander, self).__init__(name)
if not self._hand_h:
self._tactiles = TactileReceiver(prefix)
# appends trailing slash if necessary
self._topic_prefix = prefix
if self._topic_prefix and self._topic_prefix.endswith("_"):
self._topic_prefix = self._topic_prefix[:-1] # Remove trailing _
if self._topic_prefix and not self._topic_prefix.endswith("/"):
self._topic_prefix += "/"
self._hand_serial = hand_serial
def get_hand_serial(self):
return self._hand_serial
def get_joints_effort(self):
"""
Returns joints effort
@return - dictionary with joints efforts
"""
return self._get_joints_effort()
def set_max_force(self, joint_name, value):
"""
Set maximum force for hand
@param value - maximum force value
"""
joint_name = self._strip_prefix(joint_name)
if not self.__set_force_srv.get(joint_name):
service_name = "sr_hand_robot/" + self._topic_prefix + \
"change_force_PID_" + joint_name.upper()
self.__set_force_srv[joint_name] = \
rospy.ServiceProxy(service_name,
ForceController)
# get the current settings for the motor
motor_settings = None
try:
motor_settings = rospy.get_param(self._topic_prefix +
joint_name.lower() + "/pid")
except KeyError as e:
rospy.logerr("Couldn't get the motor parameters for joint " +
joint_name + " -> " + str(e))
motor_settings["torque_limit"] = value
try:
# reorder parameters in the expected order since names don't match:
self.__set_force_srv[joint_name](motor_settings["max_pwm"],
motor_settings["sg_left"],
motor_settings["sg_right"],
motor_settings["f"],
motor_settings["p"],
motor_settings["i"],
motor_settings["d"],
motor_settings["imax"],
motor_settings["deadband"],
motor_settings["sign"],
motor_settings["torque_limit"],
motor_settings["torque_limiter_gain"])
except rospy.ServiceException as e:
rospy.logerr("Couldn't set the max force for joint " +
joint_name + ": " + str(e))
def get_tactile_type(self):
"""
Returns a string indicating the type of tactile sensors present.
Possible values are: PST, biotac, UBI0.
"""
return self._tactiles.get_tactile_type()
def get_tactile_state(self):
"""
Returns an object containing tactile data. The structure of the
data is different for every tactile_type.
"""
return self._tactiles.get_tactile_state()
def _strip_prefix(self, joint_name):
"""
Strips the prefix from the joint name (e.g. rh_ffj3 -> ffj3) if present, returns the joint name otherwise.
We know that all joint names for the shadow hands are 4 char long. So we only keep the last 4 chars.
@param joint_name the joint name
@return stripped joint name
"""
return joint_name[-4:]
def attach_object(self, object_name):
self._move_group_commander.attach_object(object_name)
def detach_object(self, object_name):
self._move_group_commander.detach_object(object_name)