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)
#!/usr/bin/env python # # Convert the old style XML grasps file to new style YAML. # # rosrun sr_grasp convert_xml.py sr_hand/scripts/sr_hand/grasps.xml > converted.yaml # import sys import genpy from sr_hand.grasps_parser import GraspParser import sr_grasp from sr_robot_msgs.msg import GraspArray parser = GraspParser() parser.parse_tree(xml_filename=sys.argv[1]) grasps = GraspArray() for name, g in parser.grasps.iteritems(): ng = sr_grasp.Grasp() ng.id = name ng.set_grasp_point(positions=g.joints_and_positions) grasps.grasps.append(ng) # print grasps # oddly fails, but this works. print genpy.message.strify_message(grasps.grasps)
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()
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() #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, latch=True) self.pub_arm = rospy.Publisher('sr_arm/sendupdate', sendupdate, 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() threading.Thread(None, rospy.spin)