23 import rosgraph.masterapi
24 import control_msgs.msg
25 from sr_robot_msgs.msg
import sendupdate, joint, joints_data, JointControllerState
26 from sensor_msgs.msg
import JointState
27 from std_msgs.msg
import Float64
33 from dynamic_reconfigure.msg
import Config
38 def __init__(self, name="", motor="", j_min=0, j_max=90):
48 This is a python library used to easily access the shadow hand ROS interface. 53 Builds the library, creates the communication node in ROS and 54 initializes the hand publisher and subscriber to the default 55 values of shadowhand_data and sendupdate 58 Joint(
"THJ2",
"smart_motor_th2", -40, 40),
59 Joint(
"THJ3",
"smart_motor_th3", -15, 15),
60 Joint(
"THJ4",
"smart_motor_th4", 0, 75),
61 Joint(
"THJ5",
"smart_motor_th5", -60, 60),
62 Joint(
"FFJ0",
"smart_motor_ff2", 0, 180),
63 Joint(
"FFJ3",
"smart_motor_ff3"),
64 Joint(
"FFJ4",
"smart_motor_ff4", -20, 20),
65 Joint(
"MFJ0",
"smart_motor_mf2", 0, 180),
66 Joint(
"MFJ3",
"smart_motor_mf3"),
67 Joint(
"MFJ4",
"smart_motor_mf4", -20, 20),
68 Joint(
"RFJ0",
"smart_motor_rf2", 0, 180),
69 Joint(
"RFJ3",
"smart_motor_rf3"),
70 Joint(
"RFJ4",
"smart_motor_rf4", -20, 20),
71 Joint(
"LFJ0",
"smart_motor_lf2", 0, 180),
72 Joint(
"LFJ3",
"smart_motor_lf3"),
73 Joint(
"LFJ4",
"smart_motor_lf4", -20, 20),
74 Joint(
"LFJ5",
"smart_motor_lf5", 0, 45),
75 Joint(
"WRJ1",
"smart_motor_wr1", -45, 30),
76 Joint(
"WRJ2",
"smart_motor_wr2", -30, 10),
80 Joint(
"ShoulderJSwing",
"", 0, 80),
81 Joint(
"ElbowJSwing",
"", 0, 120),
82 Joint(
"ElbowJRotate",
"", -80, 80)
118 self.
rootPath = rospkg.RosPack().get_path(
'sr_hand')
120 self.
rootPath +
"/scripts/sr_hand/grasps.xml")
123 self.
pub = rospy.Publisher(
124 'srh/sendupdate', sendupdate, queue_size=1, latch=
True)
126 'sr_arm/sendupdate', sendupdate, queue_size=1, latch=
True)
129 'sr_arm/shadowhand_data', joints_data, self.
callback_arm)
130 self.
sub = rospy.Subscriber(
131 'srh/shadowhand_data', joints_data, self.
callback)
148 topic, Float64, queue_size=1, latch=
True)
151 threading.Thread(
None, rospy.spin)
158 @param data: The ROS message received which called the callback 159 If the message is the first received, initializes the dictionnaries 160 Else, it updates the lastMsg 161 @param joint: a Joint object that contains the name of the joint that we receive data from 163 joint_data = joint(joint_name=jointName, joint_target=math.degrees(
164 float(data.set_point)), joint_position=math.degrees(float(data.process_value)))
174 self.
lastMsg.joints_list.append(
193 @param data: The ROS message received which called the callback 194 If the message is the first received, initializes the dictionnaries 195 Else, it updates the lastMsg 200 for jnt
in self.
lastMsg.joints_list:
201 self.
dict_pos[jnt.joint_name] = jnt.joint_position
202 self.
dict_tar[jnt.joint_name] = jnt.joint_target
208 @param data: The ROS message received which called the callback 209 If the message is the first received, initializes the dictionnaries 210 Else, it updates the lastMsg 220 Initializes the library with just the fingers actually connected 223 for joint_msg
in self.
lastMsg.joints_list:
224 if joint_msg.joint_name == joint_all.name:
229 @return : true if some hand is detected 241 @return : true if the CAN hand is detected 248 "Waiting for service since " + str(t) +
" seconds...")
251 "No hand found. Are you sure the ROS hand is running ?")
257 @param topic: The new topic to be set as the hand publishing topic 258 Set the library to listen to a new topic 260 print 'Changing subscriber to ' + topic
261 self.
sub = rospy.Subscriber(topic, joints_data, self.
callback)
265 @param topic: The new topic to be set as the hand subscribing topic 266 Set the library to publish to a new topic 268 print 'Changing publisher to ' + topic
269 self.
pub = rospy.Publisher(topic, sendupdate, latch=
True)
273 @param dicti: Dictionnary containing all the targets to send, mapping 274 the name of the joint to the value of its target. 275 Sends new targets to the hand from a dictionnary 280 for join
in dicti.keys():
286 topic, Float64, latch=
True)
288 msg_to_send = Float64()
289 msg_to_send.data = math.radians(float(dicti[join]))
293 for join
in dicti.keys():
295 joint(joint_name=join, joint_target=dicti[join]))
302 @param jointName: Name of the joint to update 303 @param angle: Target of the joint, 0 if not set 304 Sends a new target for the specified joint 313 topic, Float64, latch=
True)
315 msg_to_send = Float64()
316 msg_to_send.data = math.radians(float(angle))
319 message = [joint(joint_name=jointName, joint_target=angle)]
326 @param dicti: Dictionnary containing all the targets to send, mapping 327 the name of the joint to the value of its target. 328 Sends new targets to the hand from a dictionnary 331 for join
in dicti.keys():
332 message.append(joint(joint_name=join, joint_target=dicti[join]))
337 @param jointName: Name of the joint to update 338 @param angle: Target of the joint, 0 if not set 339 Sends a new target for the specified joint 341 message = [joint(joint_name=jointName, joint_target=angle)]
346 @param jointName: Name of the joint to read the value 347 @return: 'NaN' if the value is not correct, the actual position of the joint else 349 for jnt
in self.
lastMsg.joints_list:
350 if jnt.joint_name == jointName:
351 return float(jnt.joint_position)
353 if jnt.joint_name == jointName:
354 return float(jnt.joint_position)
359 @return : True if an arm is detected on the roscore 365 master = rosgraph.masterapi.Master(
'/rostopic')
366 self.
liste = master.getPublishedTopics(
'/')
367 for topic_typ
in self.
liste:
368 for topic
in topic_typ:
369 if 'sr_arm/shadowhand_data' in topic:
375 @param filename: name (or path) of the file to save to 376 @param grasp_as_xml: xml-formatted grasp 377 Write the grasp at the end of the file, creates the file if does not exist 379 if os.path.exists(filename):
380 obj = open(filename,
'r') 381 text = obj.readlines() 383 objWrite = open(filename, 'w')
384 for index
in range(0, len(text) - 1):
385 objWrite.write(text[index])
386 objWrite.write(grasp_as_xml)
387 objWrite.write(
'</root>')
390 objWrite = open(filename,
'w')
391 objWrite.write(
'<root>\n')
392 objWrite.write(grasp_as_xml)
393 objWrite.write(
'</root>')
397 objFile = open(filename,
'w')
399 objFile.write(key +
' ' + value +
'\n')
404 @return: dictionnary mapping joint names to actual positions 405 Read all the positions in the lastMsg 409 for jnt
in self.
lastMsg.joints_list:
410 self.
dict_pos[jnt.joint_name] = jnt.joint_position
415 @return: dictionnary mapping joint names to current targets 416 Read all the targets in the lastMsg 418 for jnt
in self.
lastMsg.joints_list:
419 self.
dict_tar[jnt.joint_name] = jnt.joint_target
424 @return: dictionary mapping joint names to current velocities 431 @return: dictionary mapping joint names to current efforts 438 @return: dictionnary mapping joint names to actual positions 439 Read all the positions in the lastMsg 447 @return: dictionnary mapping joint names to actual targets 448 Read all the targets in the lastMsg 456 Resend the targets read in the lastMsg to the hand 458 for key, value
in self.
dict_tar.items():
463 @param callList: dictionnary mapping joint names to information that should be displayed 464 @param reset: flag used to tell if the parameters should be replaced by the new ones or 465 just added to the previous ones. 466 Calls a ROS service to display various information in Rviz 476 Only used to check if a real etherCAT hand is detected in the system 477 check if something is being published to this topic, otherwise 479 Bear in mind that the gazebo hand also publishes the joint_states topic, 480 so we need to check for the gazebo hand first 484 rospy.wait_for_message(
"joint_states", JointState, timeout=0.2)
485 except rospy.ROSException:
486 rospy.logwarn(
"no message received from joint_states")
493 Only used to check if a Gazebo simulated (etherCAT protocol) hand is detected in the system 494 check if something is being published to this topic, otherwise 499 rospy.wait_for_message(
500 "gazebo/parameter_updates", Config, timeout=0.2)
501 except rospy.ROSException:
508 At the moment we just try to use the mixed position velocity controllers 517 rospy.wait_for_message(
518 topic, control_msgs.msg.JointControllerState, timeout=0.2)
521 self.
topic_ending =
"_mixed_position_velocity_controller" 524 rospy.wait_for_message(
525 topic, JointControllerState, timeout=0.2)
526 except rospy.ROSException:
530 if self.
topic_ending ==
"_mixed_position_velocity_controller":
536 control_msgs.msg.JointControllerState,
547 The callback function for the topic joint_states. 548 It will store the received joint velocity and effort information in two dictionaries 549 Velocity will be converted to degrees/s. 550 Effort units are kept as they are (currently ADC units, as no calibration is performed on the strain gauges) 552 @param joint_state: the message containing the joints data. 556 for n, v
in zip(joint_state.name, joint_state.velocity)}
558 n: e
for n, e
in zip(joint_state.name, joint_state.effort)}
560 for finger
in [
'FF',
'MF',
'RF',
'LF']:
562 if (finger +
'J1')
in dic
and (finger +
'J2')
in dic:
563 dic[finger +
'J0'] = dic[
564 finger +
'J1'] + dic[finger +
'J2']
565 del dic[finger +
'J1']
566 del dic[finger +
'J2']
def sendupdate_arm_from_dict(self, dicti)
def callback_arm(self, data)
def joint_states_callback(self, joint_state)
def __init__(self, name="", motor="", j_min=0, j_max=90)
def sendupdate_arm(self, jointName, angle=0)
def get_tactile_state(self)
def record_step_to_file(self, filename, grasp_as_xml)
def activate_etherCAT_hand(self)
def sendupdate_from_dict(self, dicti)
def read_all_current_arm_positions(self)
def check_hand_type(self)
def valueof(self, jointName)
def get_tactile_type(self)
def save_hand_position_to_file(self, filename)
def init_actual_joints(self)
def callback_ethercat_states(self, data, jointName)
def check_etherCAT_hand_presence(self)
def read_all_current_targets(self)
def read_all_current_velocities(self)
def read_all_current_arm_targets(self)
def create_grasp_interpoler(self, current_step, next_step)
def read_all_current_positions(self)
def read_all_current_efforts(self)
def check_CAN_hand_presence(self)
def callVisualisationService(self, callList=0, reset=0)
def sendupdate(self, jointName, angle=0)
def set_sendupdate_topic(self, topic)
def check_gazebo_hand_presence(self)
def set_shadowhand_data_topic(self, topic)