def callback_lf(self, data):
"""
The callback function for the little finger:
called each time the tactile sensor publishes a message.
If no action is currently running, we reset the arm and
hand to their starting position
@param data: the pressure value
"""
#if we're already performing an action, don't do anything
if not self.action_running.testandset():
return
#filters the noise: check when the finger is being pressed
if data.data < .1:
self.action_running.unlock()
return
#ok finger pressed
p = subprocess.Popen('beep')
rospy.loginfo("LF touched, going to start position.")
#wait 1s for the user to release the sensor
time.sleep(.2)
#send the start position to the hand
self.hand_publisher.publish(sendupdate(len(self.start_pos_hand), self.start_pos_hand))
#send the start position to the arm
self.arm_publisher.publish(sendupdate(len(self.start_pos_arm), self.start_pos_arm))
#wait before next possible action
time.sleep(.2)
self.action_running.unlock()
def __init__(self):
smach.State.__init__(self, outcomes=['success','failed'],
output_keys=['index_counter', 'max_counter', 'last_targets'])
self.pos = Position()
self.iteration = 0
self.opened = True
self.finger_opened = sendupdate(1, [joint(joint_name="FFJ0", joint_target=0)])
self.finger_closed = sendupdate(1, [joint(joint_name="FFJ0", joint_target=160)])
def __init__(self):
smach.State.__init__(self, outcomes=['success', 'failed'],
output_keys=['index_counter', 'max_counter', 'last_targets'])
self.pos = Position()
self.iteration = 0
self.big_muscles_start = False
self.target_1 = sendupdate(len(self.pos.big_muscles_end),
self.pos.big_muscles_end)
self.target_2 = sendupdate(len(self.pos.big_muscles_start),
self.pos.big_muscles_start)
def __init__(self):
smach.State.__init__(
self,
outcomes=['success', 'failed'],
output_keys=['index_counter', 'max_counter', 'last_targets'])
self.pos = Position()
self.iteration = 0
self.opened = True
self.finger_opened = sendupdate(
1, [joint(joint_name="FFJ0", joint_target=0)])
self.finger_closed = sendupdate(
1, [joint(joint_name="FFJ0", joint_target=160)])
def __init__(self):
smach.State.__init__(
self,
outcomes=['success', 'failed'],
output_keys=['index_counter', 'max_counter', 'last_targets'])
self.pos = Position()
self.iteration = 0
self.big_muscles_start = False
self.target_1 = sendupdate(len(self.pos.big_muscles_end),
self.pos.big_muscles_end)
self.target_2 = sendupdate(len(self.pos.big_muscles_start),
self.pos.big_muscles_start)
def lf_pressed(self, data):
"""
The little finger was pressed.
If no action is currently running, we reset the arm and
hand to their starting position
@param data: the pressure value (pdc)
"""
#if we're already performing an action, don't do anything
if not self.action_running.testandset():
return
#ok finger pressed
p = subprocess.Popen('beep')
rospy.loginfo("LF touched, going to start position.")
#wait 1s for the user to release the sensor
time.sleep(.2)
#send the start position to the hand
self.hand_publish( self.start_pos_hand )
#send the start position to the arm
self.arm_publisher.publish(sendupdate(len(self.start_pos_arm), self.start_pos_arm))
#wait before next possible action
time.sleep(.2)
self.action_running.unlock()
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 lf_pressed(self, data):
"""
The little finger was pressed.
If no action is currently running, we reset the arm and
hand to their starting position
@param data: the pressure value (pdc)
"""
#if we're already performing an action, don't do anything
if not self.action_running.testandset():
return
#ok finger pressed
p = subprocess.Popen('beep')
rospy.loginfo("LF touched, going to start position.")
#wait 1s for the user to release the sensor
time.sleep(.2)
#send the start position to the hand
self.hand_publish( self.start_pos_hand )
#send the start position to the arm
self.arm_publisher.publish(sendupdate(len(self.start_pos_arm), self.start_pos_arm))
#wait before next possible action
time.sleep(.2)
self.action_running.unlock()
def __init__(self, joints_dict):
joints_table = []
for name in joints_dict.keys():
joints_table.append(joint(joint_name = name,
joint_target = joints_dict[name]))
self.msg = sendupdate(len(joints_table), joints_table)
def __init__(self):
smach.State.__init__(
self,
outcomes=['success', 'failed'],
output_keys=['index_counter', 'max_counter', 'last_targets'])
self.pos = Position()
self.targets = sendupdate(len(self.pos.first_pos_msg),
self.pos.first_pos_msg)
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 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_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 __init__(self):
#send the start position to the hand
self.hand_publisher.publish(sendupdate(len(self.start_pos_hand), self.start_pos_hand))
#send the start position to the arm
self.arm_publisher.publish(sendupdate(len(self.start_pos_arm), self.start_pos_arm))
#wait for the node to be initialized and then go to the starting position
time.sleep(1)
rospy.loginfo("OK, ready for the demo")
# We have one subscriber per tactile sensor
# This way we can easily have a different action mapped to
# each tactile sensor
# NB: The tactile sensor on the ring finger is not mapped in this example
self.sub_ff = rospy.Subscriber("/sr_tactile/touch/ff", Float64, self.callback_ff, queue_size=1)
self.sub_mf = rospy.Subscriber("/sr_tactile/touch/mf", Float64, self.callback_mf, queue_size=1)
self.sub_lf = rospy.Subscriber("/sr_tactile/touch/lf", Float64, self.callback_lf, queue_size=1)
self.sub_th = rospy.Subscriber("/sr_tactile/touch/th", Float64, self.callback_th, queue_size=1)
def callback_mf(self, data):
"""
The callback function for the middle finger:
called each time the tactile sensor publishes a message.
If no action is currently running, we set the ShoulderJRotate
to a positive value proportional to the pressure received.
@param data: the pressure value
"""
#if we're already performing an action, don't do anything
if not self.action_running.testandset():
return
#filters the noise: check when the finger is being pressed
if data.data < .1:
self.action_running.unlock()
return
#ok finger was pressed
p = subprocess.Popen('beep')
#rotate the trunk to (data_received * min_pos)
data.data /= 2.
if data.data > 1.:
data.data = 1.
target_sh_rot = 0. + data.data * (45.0)
target_sh_swing = 20. + data.data * (20.0)
target_elb_swing = 90. + data.data * (20.0)
target_elb_rot = 0. + data.data * (45.0)
rospy.loginfo("MF touched, going to new target ")
#wait 1s for the user to release the sensor
time.sleep(.2)
message = [joint(joint_name = "ShoulderJRotate", joint_target = target_sh_rot),
joint(joint_name = "ShoulderJSwing", joint_target = target_sh_swing),
joint(joint_name = "ElbowJRotate", joint_target = target_elb_rot),
joint(joint_name = "ElbowJSwing", joint_target = target_elb_swing)
]
self.arm_publisher.publish(sendupdate(len(message), message))
#wait before next possible action
time.sleep(.2)
self.action_running.unlock()
def ff_pressed(self,data):
"""
The first finger was pressed.
If no action is currently running, we set the ShoulderJRotate
to a negative value proportional to the pressure received.
@param data: the pressure value (pdc)
"""
#if we're already performing an action, don't do anything
if not self.action_running.testandset():
return
#ok the finger sensor was pressed
p = subprocess.Popen('beep')
#rotate the trunk to (data_received * min_pos)
# convert data to be in [0., 1.]
data /= 4000.
if data > 1.:
data = 1.
target_sh_rot = 0. + data * (-45.0)
target_sh_swing = 20. + data * (-10.0)
target_elb_swing = 90. + data * (-10.0)
target_elb_rot = 0. + data * (-45.0)
rospy.loginfo("FF touched, going to new target ")
#wait 1s for the user to release the sensor
time.sleep(.2)
message = [joint(joint_name = "ShoulderJRotate", joint_target = target_sh_rot),
joint(joint_name = "ShoulderJSwing", joint_target = target_sh_swing),
joint(joint_name = "ElbowJRotate", joint_target = target_elb_rot),
joint(joint_name = "ElbowJSwing", joint_target = target_elb_swing)
]
self.arm_publisher.publish(sendupdate(len(message), message))
#send the start position to the hand
self.hand_publish( self.extended_pos_hand )
#wait before next possible action
time.sleep(.2)
self.action_running.unlock()
def ff_pressed(self,data):
"""
The first finger was pressed.
If no action is currently running, we set the ShoulderJRotate
to a negative value proportional to the pressure received.
@param data: the pressure value (pdc)
"""
#if we're already performing an action, don't do anything
if not self.action_running.testandset():
return
#ok the finger sensor was pressed
p = subprocess.Popen('beep')
#rotate the trunk to (data_received * min_pos)
# convert data to be in [0., 1.]
data /= 4000.
if data > 1.:
data = 1.
target_sh_rot = 0. + data * (-45.0)
target_sh_swing = 20. + data * (-10.0)
target_elb_swing = 90. + data * (-10.0)
target_elb_rot = 0. + data * (-45.0)
rospy.loginfo("FF touched, going to new target ")
#wait 1s for the user to release the sensor
time.sleep(.2)
message = [joint(joint_name = "ShoulderJRotate", joint_target = target_sh_rot),
joint(joint_name = "ShoulderJSwing", joint_target = target_sh_swing),
joint(joint_name = "ElbowJRotate", joint_target = target_elb_rot),
joint(joint_name = "ElbowJSwing", joint_target = target_elb_swing)
]
self.arm_publisher.publish(sendupdate(len(message), message))
#send the start position to the hand
self.hand_publish( self.extended_pos_hand )
#wait before next possible action
time.sleep(.2)
self.action_running.unlock()
def callback(data):
"""
The callback function: called each time a message is received on the
topic /srh/shadowhand_data
@param data: the message
"""
message=[]
if data.joints_list_length == 0:
return
# loop on the joints in the message
for joint_data in data.joints_list:
# if its the parent joint, read the target and send it to the child
if joint_data.joint_name == parent_name:
message.append(joint(joint_name=child_name, joint_target=joint_data.joint_target))
# publish the message to the /srh/sendupdate topic.
pub = rospy.Publisher('srh/sendupdate', sendupdate)
pub.publish(sendupdate(len(message), message))
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
"""
#print(dicti)
if (self.check_hand_type() == "etherCAT") or (self.check_hand_type() == "gazebo"):
for join in dicti.keys():
if not self.eth_publishers.has_key(join):
topic = "/sh_"+ join.lower() + self.topic_ending+"/command"
self.eth_publishers[join] = rospy.Publisher(topic, Float64)
msg_to_send = Float64()
msg_to_send.data = math.radians( float( dicti[join] ) )
self.eth_publishers[join].publish(msg_to_send)
elif self.check_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))
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.check_hand_type() == "etherCAT") or (self.check_hand_type() == "gazebo"):
if not self.eth_publishers.has_key(jointName):
topic = "/sh_"+ jointName.lower() + self.topic_ending + "/command"
self.eth_publishers[jointName] = rospy.Publisher(topic, Float64)
msg_to_send = Float64()
msg_to_send.data = math.radians( float( angle ) )
self.eth_publishers[jointName].publish(msg_to_send)
elif self.check_hand_type() == "CANhand":
message = [joint(joint_name=jointName, joint_target=angle)]
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 __init__(self):
#A vector containing the different callbacks, in the same order
# as the tactiles.
self.fingers_pressed_functions = [self.ff_pressed, self.mf_pressed, self.rf_pressed,
self.lf_pressed, self.th_pressed]
#The hand publishers:
# we use a dictionnary of publishers, because on the etherCAT hand
# you have one publisher per controller.
self.hand_publishers = self.create_hand_publishers()
#send the start position to the hand
self.hand_publish(self.start_pos_hand)
#send the start position to the arm
self.arm_publisher.publish( sendupdate(len(self.start_pos_arm), self.start_pos_arm) )
#wait for the node to be initialized and then go to the starting position
time.sleep(1)
rospy.loginfo("OK, ready for the demo")
# We subscribe to the data being published by the biotac sensors.
self.sub_biotacs = rospy.Subscriber("tactiles", BiotacAll, self.callback_biotacs, queue_size=1)
self.sub_psts = rospy.Subscriber("tactile", ShadowPST, self.callback_psts, queue_size=1)
def __init__(self):
#A vector containing the different callbacks, in the same order
# as the tactiles.
self.fingers_pressed_functions = [self.ff_pressed, self.mf_pressed, self.rf_pressed,
self.lf_pressed, self.th_pressed]
#The hand publishers:
# we use a dictionnary of publishers, because on the etherCAT hand
# you have one publisher per controller.
self.hand_publishers = self.create_hand_publishers()
#send the start position to the hand
self.hand_publish(self.start_pos_hand)
#send the start position to the arm
self.arm_publisher.publish( sendupdate(len(self.start_pos_arm), self.start_pos_arm) )
#wait for the node to be initialized and then go to the starting position
time.sleep(1)
rospy.loginfo("OK, ready for the demo")
# We subscribe to the data being published by the biotac sensors.
self.sub_biotacs = rospy.Subscriber("/tactiles", BiotacAll, self.callback_biotacs, queue_size=1)
self.sub_psts = rospy.Subscriber("/tactile", ShadowPST, self.callback_psts, queue_size=1)
def talker():
"""
The Publisher publishes to the different topics on which the sr_subscriber subscribes. It sends commands to
the hand.
Please set the message you want to test.
"""
test_what = "sendupdate" # choose sendupdate or contrlr
"""
Test the sendupdate command
"""
if test_what == "sendupdate":
pub1 = rospy.Publisher('srh/sendupdate', sendupdate)
rospy.init_node('shadowhand_command_publisher_python')
new_target = 0
time.sleep(1)
print "publishing"
data_to_send = [ joint(joint_name="FFJ0", joint_target=new_target),
joint(joint_name="FFJ3", joint_target=new_target) ]
pub1.publish(sendupdate(len(data_to_send), data_to_send))
"""
Tests the contrlr command
"""
if test_what == "contrlr":
pub2 = rospy.Publisher('contrlr', contrlr)
data_to_send = ["p:0","i:0"]
pub2.publish( contrlr( contrlr_name="smart_motor_ff2" ,
list_of_parameters = data_to_send,
length_of_list = len(data_to_send) ) )
def __init__(self):
smach.State.__init__(self, outcomes=['success','failed'],
output_keys=['index_counter', 'max_counter', 'last_targets'])
self.pos = Position()
self.targets = sendupdate(len(self.pos.first_pos_msg),
self.pos.first_pos_msg)
'ffj3': 0,
'ffj4': 0,
'rfj0': 40.0,
'rfj3': 0,
'rfj4': 0,
'thj1': 20,
'thj2': 31,
'thj3': 0,
'thj4': 25,
'thj5': -29,
'wrj1': 0,
'wrj2': 0
}
print "Start"
sendupdate(start_pose)
rospy.sleep(2)
# FF curl
sendupdate({'ffj3': 90})
rospy.sleep(3)
sendupdate({'ffj0': 120})
rospy.sleep(2)
sendupdate(start_pose)
rospy.sleep(4)
# RF curl
sendupdate({'rfj0': 120, 'rfj3': 90})
rospy.sleep(4)
sendupdate(start_pose)
rospy.sleep(4)
