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