def order_joint_states(self, data):
"""
Fetch the joint state from a service call.
Joints positions are then stored in the same way of the "joint" msg structure.
This will help for comparing current and target position.
@ param data: joint state in the form of a "JointState" msg
"""
ordered_joint = []
# Create a dictionary that contains joint name and position
# starting from a JointState msg
joints_dictionary = dict(
zip(data.joint_state.name, data.joint_state.position))
# helper variable
suffix = 'FJ0'
# For each joint name, look for the corresponding value in the joints_dictionary
for key in [
"FFJ0", "FFJ3", "FFJ4", "MFJ0", "MFJ3", "MFJ4", "RFJ0", "RFJ3",
"RFJ4", "LFJ0", "LFJ3", "LFJ4", "LFJ5", "THJ1", "THJ2", "THJ3",
"THJ4", "THJ5", "WRJ1", "WRJ2"
]:
# Check if the key contains "FJ0": "FFJ0", "MFJ0", "RFJ0", "LFJ0"
# This has to be done because for convention ?FJO = ?FJ1 + ?FJ2
if key[1:] == suffix:
ordered_joint.append(
joint(joint_name=key,
joint_target=joints_dictionary[key[:1] + "FJ1"] +
joints_dictionary[key[:1] + "FJ2"]))
else:
ordered_joint.append(
joint(joint_name=key, joint_target=joints_dictionary[key]))
return ordered_joint
def order_joint_states(self, data):
"""
Fetch the joint state from a service call.
Joints positions are then stored in the same way of the "joint" msg structure.
This will help for comparing current and target position.
@ param data: joint state in the form of a "JointState" msg
"""
ordered_joint = []
# Create a dictionary that contains joint name and position
# starting from a JointState msg
joints_dictionary = dict(zip(data.joint_state.name, data.joint_state.position))
# helper variable
suffix = 'FJ0'
# For each joint name, look for the corresponding value in the joints_dictionary
for key in ["FFJ0", "FFJ3", "FFJ4",
"MFJ0", "MFJ3", "MFJ4",
"RFJ0", "RFJ3", "RFJ4",
"LFJ0", "LFJ3", "LFJ4", "LFJ5",
"THJ1", "THJ2", "THJ3", "THJ4", "THJ5",
"WRJ1", "WRJ2" ]:
# Check if the key contains "FJ0": "FFJ0", "MFJ0", "RFJ0", "LFJ0"
# This has to be done because for convention ?FJO = ?FJ1 + ?FJ2
if key[1:] == suffix:
ordered_joint.append( joint(joint_name = key,
joint_target = joints_dictionary[key[:1]+"FJ1"] + joints_dictionary[key[:1]+"FJ2"]))
else:
ordered_joint.append( joint(joint_name = key,
joint_target = joints_dictionary[key]) )
return ordered_joint
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.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 joints_data(cls, msg, obj):
obj.joints_list_length = msg["joints_list_length"]
for i in range(msg['_length']):
j = joint()
j = decode.joint(msg, j, i)
obj.joints_list.append(j)
return(obj)
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_big_muscles_end(self):
pos = {
'FFJ0': 160,
'FFJ3': 80,
'FFJ4': 0,
'MFJ0': 160,
'MFJ3': 80,
'MFJ4': 0,
'RFJ0': 160,
'RFJ3': 80,
'RFJ4': 0,
'LFJ0': 160,
'LFJ3': 80,
'LFJ4': 0,
'LFJ5': 0,
'WRJ1': 30,
'WRJ2': 0,
'THJ1': 45,
'THJ2': 20,
'THJ3': 0,
'THJ4': 50,
'THJ5': 5,
'ShoulderJRotate': -30,
'ShoulderJSwing': 20,
'ElbowJSwing': 120,
'ElbowJRotate': -80
}
for joint_name in pos.keys():
self.big_muscles_end.append(
joint(joint_name=joint_name, joint_target=pos[joint_name]))
def init_first_pos_msg(self):
pos = {
'FFJ0': 0,
'FFJ3': 0,
'FFJ4': 0,
'MFJ0': 160,
'MFJ3': 80,
'MFJ4': 0,
'RFJ0': 160,
'RFJ3': 80,
'RFJ4': 0,
'LFJ0': 160,
'LFJ3': 80,
'LFJ4': 0,
'LFJ5': 0,
'WRJ1': 0,
'WRJ2': 0,
'THJ1': 45,
'THJ2': 20,
'THJ3': 0,
'THJ4': 50,
'THJ5': 5,
'ShoulderJRotate': -45,
'ShoulderJSwing': 10,
'ElbowJSwing': 120,
'ElbowJRotate': -80
}
for joint_name in pos.keys():
self.first_pos_msg.append(
joint(joint_name=joint_name, joint_target=pos[joint_name]))
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
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 send_update(cls, msg, obj):
obj.sendupdate_length = msg["sendupdate_length"]
for i in range(msg['_length']):
j = joint()
j = decode.joint(msg, j, i)
obj.sendupdate_list.append(j)
return(obj)
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_arm(len(message), message))
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 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 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 command(cls, msg, obj):
obj.command_type = msg["command_type"]
obj.sendupdate_command.sendupdate_length = msg["sendupdate_length"]
for i in range(msg['_length']):
j = joint()
j = decode.joint(msg, j, i)
obj.sendupdate_command.sendupdate_list.append(j)
obj.contrlr_command.contrlr_name = msg["contrlr_name"]
obj.contrlr_command.list_of_parameters = msg["list_of_parameters"]
obj.contrlr_command.length_of_list = msg["length_of_list"]
return(obj)
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 fetch_target(self, name):
"""
Fetch the targets for the etherCAT hand from the server parameter
"""
target_name = []
for x in ["FFJ0", "FFJ3", "FFJ4",
"MFJ0", "MFJ3", "MFJ4",
"RFJ0", "RFJ3", "RFJ4",
"LFJ0", "LFJ3", "LFJ4", "LFJ5",
"THJ1", "THJ2", "THJ3", "THJ4", "THJ5",
"WRJ1", "WRJ2" ]:
target_name.append( joint(joint_name = x,
joint_target = rospy.get_param('/targets/'+name+'/'+x)) )
return target_name
def fetch_target(self, name):
"""
Fetch the targets for the etherCAT hand from the server parameter
"""
target_name = []
for x in [
"FFJ0", "FFJ3", "FFJ4", "MFJ0", "MFJ3", "MFJ4", "RFJ0", "RFJ3",
"RFJ4", "LFJ0", "LFJ3", "LFJ4", "LFJ5", "THJ1", "THJ2", "THJ3",
"THJ4", "THJ5", "WRJ1", "WRJ2"
]:
target_name.append(
joint(joint_name=x,
joint_target=rospy.get_param('/targets/' + name + '/' +
x)))
return target_name
def init_big_muscles_end(self):
pos = {'FFJ0': 160, 'FFJ3': 80, 'FFJ4': 0,
'MFJ0': 160, 'MFJ3': 80, 'MFJ4': 0,
'RFJ0': 160, 'RFJ3': 80, 'RFJ4': 0,
'LFJ0': 160, 'LFJ3': 80, 'LFJ4': 0, 'LFJ5': 0,
'WRJ1': 30, 'WRJ2': 0,
'THJ1': 45, 'THJ2': 20, 'THJ3': 0, 'THJ4': 50, 'THJ5': 5,
'ShoulderJRotate': -30,
'ShoulderJSwing': 20,
'ElbowJSwing': 120,
'ElbowJRotate': -80
}
for joint_name in pos.keys():
self.big_muscles_end.append(joint(joint_name=joint_name,
joint_target=pos[joint_name]))
def init_first_pos_msg(self):
pos = {'FFJ0': 0, 'FFJ3': 0, 'FFJ4': 0,
'MFJ0': 160, 'MFJ3': 80, 'MFJ4': 0,
'RFJ0': 160, 'RFJ3': 80, 'RFJ4': 0,
'LFJ0': 160, 'LFJ3': 80, 'LFJ4': 0, 'LFJ5': 0,
'WRJ1': 0, 'WRJ2': 0,
'THJ1': 45, 'THJ2': 20, 'THJ3': 0, 'THJ4': 50, 'THJ5': 5,
'ShoulderJRotate': -45,
'ShoulderJSwing': 10,
'ElbowJSwing': 120,
'ElbowJRotate': -80
}
for joint_name in pos.keys():
self.first_pos_msg.append(joint(joint_name=joint_name,
joint_target=pos[joint_name]))
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 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
'''
# any later version.
#
# This program is distributed in the hope that it will be useful, but WITHOUT
# ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
# FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
# more details.
#
# You should have received a copy of the GNU General Public License along
# with this program. If not, see <http://www.gnu.org/licenses/>.
#
from sr_robot_msgs.msg import joint
# start position for counting: punch
punch_step1 = [
joint(joint_name="THJ1", joint_target=68),
joint(joint_name="THJ2", joint_target=30),
joint(joint_name="THJ3", joint_target=15),
joint(joint_name="THJ4", joint_target=70),
joint(joint_name="THJ5", joint_target=30),
joint(joint_name="FFJ0", joint_target=0),
joint(joint_name="FFJ3", joint_target=0),
joint(joint_name="FFJ4", joint_target=0),
joint(joint_name="MFJ0", joint_target=0),
joint(joint_name="MFJ3", joint_target=0),
joint(joint_name="MFJ4", joint_target=0),
joint(joint_name="RFJ0", joint_target=0),
joint(joint_name="RFJ3", joint_target=0),
joint(joint_name="RFJ4", joint_target=0),
joint(joint_name="LFJ0", joint_target=0),
joint(joint_name="LFJ3", joint_target=0),
class FancyDemo(object):
# starting position for the hand (DON't use until reviewed. Should be executed in two movement sequences)
start_pos_hand = [
joint(joint_name="rh_THJ1", joint_target=0),
joint(joint_name="rh_THJ2", joint_target=0),
joint(joint_name="rh_THJ3", joint_target=0),
joint(joint_name="rh_THJ4", joint_target=0),
joint(joint_name="rh_THJ5", joint_target=0),
joint(joint_name="rh_FFJ0", joint_target=0),
joint(joint_name="rh_FFJ3", joint_target=0),
joint(joint_name="rh_FFJ4", joint_target=0),
joint(joint_name="rh_MFJ0", joint_target=0),
joint(joint_name="rh_MFJ3", joint_target=0),
joint(joint_name="rh_MFJ4", joint_target=0),
joint(joint_name="rh_RFJ0", joint_target=0),
joint(joint_name="rh_RFJ3", joint_target=0),
joint(joint_name="rh_RFJ4", joint_target=0),
joint(joint_name="rh_LFJ0", joint_target=0),
joint(joint_name="rh_LFJ3", joint_target=0),
joint(joint_name="rh_LFJ4", joint_target=0),
joint(joint_name="rh_LFJ5", joint_target=0),
joint(joint_name="rh_WRJ1", joint_target=0),
joint(joint_name="rh_WRJ2", joint_target=0)
]
# flex first finger
flex_ff = [
joint(joint_name="rh_FFJ0", joint_target=180),
joint(joint_name="rh_FFJ3", joint_target=90),
joint(joint_name="rh_FFJ4", joint_target=0)
]
# extend first finger
ext_ff = [
joint(joint_name="rh_FFJ0", joint_target=0),
joint(joint_name="rh_FFJ3", joint_target=0),
joint(joint_name="rh_FFJ4", joint_target=0)
]
# flex middle finger
flex_mf = [
joint(joint_name="rh_MFJ0", joint_target=180),
joint(joint_name="rh_MFJ3", joint_target=90),
joint(joint_name="rh_MFJ4", joint_target=0)
]
# extend middle finger
ext_mf = [
joint(joint_name="rh_MFJ0", joint_target=0),
joint(joint_name="rh_MFJ3", joint_target=0),
joint(joint_name="rh_MFJ4", joint_target=0)
]
# flex ring finger
flex_rf = [
joint(joint_name="rh_RFJ0", joint_target=180),
joint(joint_name="rh_RFJ3", joint_target=90),
joint(joint_name="rh_RFJ4", joint_target=0)
]
# extend ring finger
ext_rf = [
joint(joint_name="rh_RFJ0", joint_target=0),
joint(joint_name="rh_RFJ3", joint_target=0),
joint(joint_name="rh_RFJ4", joint_target=0)
]
# flex little finger
flex_lf = [
joint(joint_name="rh_LFJ0", joint_target=180),
joint(joint_name="rh_LFJ3", joint_target=90),
joint(joint_name="rh_LFJ4", joint_target=0)
]
# extend little finger
ext_lf = [
joint(joint_name="rh_LFJ0", joint_target=0),
joint(joint_name="rh_LFJ3", joint_target=0),
joint(joint_name="rh_LFJ4", joint_target=0)
]
# flex thumb step 1
flex_th_1 = [
joint(joint_name="rh_THJ1", joint_target=0),
joint(joint_name="rh_THJ2", joint_target=0),
joint(joint_name="rh_THJ3", joint_target=0),
joint(joint_name="rh_THJ4", joint_target=70),
joint(joint_name="rh_THJ5", joint_target=0)
]
# flex thumb step 2
flex_th_2 = [
joint(joint_name="rh_THJ1", joint_target=70),
joint(joint_name="rh_THJ2", joint_target=26),
joint(joint_name="rh_THJ3", joint_target=0),
joint(joint_name="rh_THJ4", joint_target=70),
joint(joint_name="rh_THJ5", joint_target=50)
]
# extend thumb step 1
ext_th_1 = [
joint(joint_name="rh_THJ1", joint_target=70),
joint(joint_name="rh_THJ2", joint_target=15),
joint(joint_name="rh_THJ3", joint_target=0),
joint(joint_name="rh_THJ4", joint_target=70),
joint(joint_name="rh_THJ5", joint_target=0)
]
# extend thumb step 2
ext_th_2 = [
joint(joint_name="rh_THJ1", joint_target=0),
joint(joint_name="rh_THJ2", joint_target=0),
joint(joint_name="rh_THJ3", joint_target=-12),
joint(joint_name="rh_THJ4", joint_target=0),
joint(joint_name="rh_THJ5", joint_target=-50)
]
# zero thumb
zero_th = [
joint(joint_name="rh_THJ1", joint_target=0),
joint(joint_name="rh_THJ2", joint_target=0),
joint(joint_name="rh_THJ3", joint_target=0),
joint(joint_name="rh_THJ4", joint_target=0),
joint(joint_name="rh_THJ5", joint_target=0)
]
# Pre O.K. with first finger
pre_ff_ok = [joint(joint_name="rh_THJ4", joint_target=50)]
# O.K. with first finger
ff_ok = [
joint(joint_name="rh_FFJ0", joint_target=93),
joint(joint_name="rh_FFJ3", joint_target=37),
joint(joint_name="rh_FFJ4", joint_target=-0.2),
joint(joint_name="rh_MFJ0", joint_target=42),
joint(joint_name="rh_MFJ3", joint_target=33),
joint(joint_name="rh_MFJ4", joint_target=-3),
joint(joint_name="rh_RFJ0", joint_target=50),
joint(joint_name="rh_RFJ3", joint_target=18),
joint(joint_name="rh_RFJ4", joint_target=0.5),
joint(joint_name="rh_LFJ0", joint_target=30),
joint(joint_name="rh_LFJ3", joint_target=0),
joint(joint_name="rh_LFJ4", joint_target=-6),
joint(joint_name="rh_LFJ5", joint_target=7),
joint(joint_name="rh_THJ1", joint_target=40),
joint(joint_name="rh_THJ2", joint_target=12),
joint(joint_name="rh_THJ3", joint_target=-10),
joint(joint_name="rh_THJ4", joint_target=50),
joint(joint_name="rh_THJ5", joint_target=11)
]
# O.K. transition from first finger to middle finger
ff2mf_ok = [
joint(joint_name="rh_FFJ0", joint_target=13.6),
joint(joint_name="rh_FFJ3", joint_target=7),
joint(joint_name="rh_FFJ4", joint_target=-0.4),
joint(joint_name="rh_MFJ0", joint_target=42),
joint(joint_name="rh_MFJ3", joint_target=33),
joint(joint_name="rh_MFJ4", joint_target=-3),
joint(joint_name="rh_RFJ0", joint_target=50),
joint(joint_name="rh_RFJ3", joint_target=18),
joint(joint_name="rh_RFJ4", joint_target=0.5),
joint(joint_name="rh_LFJ0", joint_target=30),
joint(joint_name="rh_LFJ3", joint_target=0),
joint(joint_name="rh_LFJ4", joint_target=-6),
joint(joint_name="rh_LFJ5", joint_target=7),
joint(joint_name="rh_THJ1", joint_target=40),
joint(joint_name="rh_THJ2", joint_target=12),
joint(joint_name="rh_THJ3", joint_target=-10),
joint(joint_name="rh_THJ4", joint_target=50),
joint(joint_name="rh_THJ5", joint_target=2)
]
# O.K. with middle finger
mf_ok = [
joint(joint_name="rh_FFJ0", joint_target=13.6),
joint(joint_name="rh_FFJ3", joint_target=7),
joint(joint_name="rh_FFJ4", joint_target=-0.4),
joint(joint_name="rh_MFJ0", joint_target=89),
joint(joint_name="rh_MFJ3", joint_target=45),
joint(joint_name="rh_MFJ4", joint_target=8),
joint(joint_name="rh_RFJ0", joint_target=50),
joint(joint_name="rh_RFJ3", joint_target=18),
joint(joint_name="rh_RFJ4", joint_target=0.5),
joint(joint_name="LFJ0", joint_target=30),
joint(joint_name="LFJ3", joint_target=0),
joint(joint_name="LFJ4", joint_target=-6),
joint(joint_name="LFJ5", joint_target=7),
joint(joint_name="rh_THJ1", joint_target=45),
joint(joint_name="rh_THJ2", joint_target=7),
joint(joint_name="rh_THJ3", joint_target=-10),
joint(joint_name="rh_THJ4", joint_target=66),
joint(joint_name="rh_THJ5", joint_target=23)
]
# O.K. transition from middle finger to ring finger
mf2rf_ok = [
joint(joint_name="rh_FFJ0", joint_target=13.6),
joint(joint_name="rh_FFJ3", joint_target=7),
joint(joint_name="rh_FFJ4", joint_target=-0.4),
joint(joint_name="rh_MFJ0", joint_target=45),
joint(joint_name="rh_MFJ3", joint_target=3.7),
joint(joint_name="rh_MFJ4", joint_target=-1),
joint(joint_name="rh_RFJ0", joint_target=50),
joint(joint_name="rh_RFJ3", joint_target=18),
joint(joint_name="rh_RFJ4", joint_target=0.5),
joint(joint_name="LFJ0", joint_target=30),
joint(joint_name="LFJ3", joint_target=0),
joint(joint_name="LFJ4", joint_target=-6),
joint(joint_name="LFJ5", joint_target=7),
joint(joint_name="rh_THJ1", joint_target=45),
joint(joint_name="rh_THJ2", joint_target=8),
joint(joint_name="rh_THJ3", joint_target=-9.2),
joint(joint_name="rh_THJ4", joint_target=64),
joint(joint_name="rh_THJ5", joint_target=13.2)
]
# O.K. with ring finger
rf_ok = [
joint(joint_name="rh_FFJ0", joint_target=13.6),
joint(joint_name="rh_FFJ3", joint_target=7),
joint(joint_name="rh_FFJ4", joint_target=-0.4),
joint(joint_name="rh_MFJ0", joint_target=45),
joint(joint_name="rh_MFJ3", joint_target=3.7),
joint(joint_name="rh_MFJ4", joint_target=-1),
joint(joint_name="rh_RFJ0", joint_target=90),
joint(joint_name="rh_RFJ3", joint_target=48),
joint(joint_name="rh_RFJ4", joint_target=-20),
joint(joint_name="LFJ0", joint_target=30),
joint(joint_name="LFJ3", joint_target=0),
joint(joint_name="LFJ4", joint_target=-6),
joint(joint_name="LFJ5", joint_target=7),
joint(joint_name="rh_THJ1", joint_target=44),
joint(joint_name="rh_THJ2", joint_target=8),
joint(joint_name="rh_THJ3", joint_target=15),
joint(joint_name="rh_THJ4", joint_target=70),
joint(joint_name="rh_THJ5", joint_target=27)
]
# O.K. transition from ring finger to little finger
rf2lf_ok = [
joint(joint_name="rh_FFJ0", joint_target=13.6),
joint(joint_name="rh_FFJ3", joint_target=7),
joint(joint_name="rh_FFJ4", joint_target=-0.4),
joint(joint_name="rh_MFJ0", joint_target=45),
joint(joint_name="rh_MFJ3", joint_target=3.7),
joint(joint_name="rh_MFJ4", joint_target=-1),
joint(joint_name="rh_RFJ0", joint_target=74),
joint(joint_name="rh_RFJ3", joint_target=6.5),
joint(joint_name="rh_RFJ4", joint_target=0.5),
joint(joint_name="rh_LFJ0", joint_target=30),
joint(joint_name="rh_LFJ3", joint_target=0),
joint(joint_name="rh_LFJ4", joint_target=-6),
joint(joint_name="rh_LFJ5", joint_target=7),
joint(joint_name="rh_THJ1", joint_target=42),
joint(joint_name="rh_THJ2", joint_target=4.5),
joint(joint_name="rh_THJ3", joint_target=7.7),
joint(joint_name="rh_THJ4", joint_target=73),
joint(joint_name="rh_THJ5", joint_target=21)
]
# O.K. with little finger
lf_ok = [
joint(joint_name="rh_FFJ0", joint_target=13.6),
joint(joint_name="rh_FFJ3", joint_target=7),
joint(joint_name="rh_FFJ4", joint_target=-0.4),
joint(joint_name="rh_MFJ0", joint_target=15),
joint(joint_name="rh_MFJ3", joint_target=3.7),
joint(joint_name="rh_MFJ4", joint_target=-1),
joint(joint_name="rh_RFJ0", joint_target=74),
joint(joint_name="rh_RFJ3", joint_target=6.5),
joint(joint_name="rh_RFJ4", joint_target=0.5),
joint(joint_name="rh_LFJ0", joint_target=100),
joint(joint_name="rh_LFJ3", joint_target=9),
joint(joint_name="rh_LFJ4", joint_target=-7.6),
joint(joint_name="rh_LFJ5", joint_target=37),
joint(joint_name="rh_THJ1", joint_target=40),
joint(joint_name="rh_THJ2", joint_target=10),
joint(joint_name="rh_THJ3", joint_target=10),
joint(joint_name="rh_THJ4", joint_target=68),
joint(joint_name="rh_THJ5", joint_target=25)
]
# zero wrist
zero_wr = [
joint(joint_name="rh_WRJ1", joint_target=0),
joint(joint_name="rh_WRJ2", joint_target=0)
]
# north wrist
n_wr = [
joint(joint_name="rh_WRJ1", joint_target=15),
joint(joint_name="rh_WRJ2", joint_target=0)
]
# south wrist
s_wr = [
joint(joint_name="rh_WRJ1", joint_target=-20),
joint(joint_name="rh_WRJ2", joint_target=0)
]
# east wrist
e_wr = [
joint(joint_name="rh_WRJ1", joint_target=0),
joint(joint_name="rh_WRJ2", joint_target=8)
]
# west wrist
w_wr = [
joint(joint_name="rh_WRJ1", joint_target=0),
joint(joint_name="rh_WRJ2", joint_target=-14)
]
# northeast wrist
ne_wr = [
joint(joint_name="rh_WRJ1", joint_target=15),
joint(joint_name="rh_WRJ2", joint_target=8)
]
# northwest wrist
nw_wr = [
joint(joint_name="rh_WRJ1", joint_target=15),
joint(joint_name="rh_WRJ2", joint_target=-14)
]
# southweast wrist
sw_wr = [
joint(joint_name="rh_WRJ1", joint_target=-20),
joint(joint_name="rh_WRJ2", joint_target=-14)
]
# southeast wrist
se_wr = [
joint(joint_name="rh_WRJ1", joint_target=-20),
joint(joint_name="rh_WRJ2", joint_target=8)
]
# grasp for shaking hands step 1
shake_grasp_1 = [
joint(joint_name="rh_THJ1", joint_target=0),
joint(joint_name="rh_THJ2", joint_target=6),
joint(joint_name="rh_THJ3", joint_target=10),
joint(joint_name="rh_THJ4", joint_target=37),
joint(joint_name="rh_THJ5", joint_target=9),
joint(joint_name="rh_FFJ0", joint_target=21),
joint(joint_name="rh_FFJ3", joint_target=26),
joint(joint_name="rh_FFJ4", joint_target=0),
joint(joint_name="rh_MFJ0", joint_target=18),
joint(joint_name="rh_MFJ3", joint_target=24),
joint(joint_name="rh_MFJ4", joint_target=0),
joint(joint_name="rh_RFJ0", joint_target=30),
joint(joint_name="rh_RFJ3", joint_target=16),
joint(joint_name="rh_RFJ4", joint_target=0),
joint(joint_name="rh_LFJ0", joint_target=30),
joint(joint_name="rh_LFJ3", joint_target=0),
joint(joint_name="rh_LFJ4", joint_target=-10),
joint(joint_name="rh_LFJ5", joint_target=10)
]
# grasp for shaking hands step 2
shake_grasp_2 = [
joint(joint_name="rh_THJ1", joint_target=21),
joint(joint_name="rh_THJ2", joint_target=12),
joint(joint_name="rh_THJ3", joint_target=10),
joint(joint_name="rh_THJ4", joint_target=42),
joint(joint_name="rh_THJ5", joint_target=21),
joint(joint_name="rh_FFJ0", joint_target=75),
joint(joint_name="rh_FFJ3", joint_target=29),
joint(joint_name="rh_FFJ4", joint_target=0),
joint(joint_name="rh_MFJ0", joint_target=85),
joint(joint_name="rh_MFJ3", joint_target=29),
joint(joint_name="rh_MFJ4", joint_target=0),
joint(joint_name="rh_RFJ0", joint_target=75),
joint(joint_name="rh_RFJ3", joint_target=41),
joint(joint_name="rh_RFJ4", joint_target=0),
joint(joint_name="rh_LFJ0", joint_target=100),
joint(joint_name="rh_LFJ3", joint_target=41),
joint(joint_name="rh_LFJ4", joint_target=0),
joint(joint_name="rh_LFJ5", joint_target=0)
]
# store step 1
store_1 = [
joint(joint_name="rh_THJ1", joint_target=0),
joint(joint_name="rh_THJ2", joint_target=0),
joint(joint_name="rh_THJ3", joint_target=0),
joint(joint_name="rh_THJ4", joint_target=65),
joint(joint_name="rh_THJ5", joint_target=0),
joint(joint_name="rh_FFJ0", joint_target=180),
joint(joint_name="rh_FFJ3", joint_target=90),
joint(joint_name="rh_FFJ4", joint_target=0),
joint(joint_name="rh_MFJ0", joint_target=180),
joint(joint_name="rh_MFJ3", joint_target=90),
joint(joint_name="rh_MFJ4", joint_target=0),
joint(joint_name="rh_RFJ0", joint_target=180),
joint(joint_name="rh_RFJ3", joint_target=90),
joint(joint_name="rh_RFJ4", joint_target=0),
joint(joint_name="rh_LFJ0", joint_target=180),
joint(joint_name="rh_LFJ3", joint_target=90),
joint(joint_name="rh_LFJ4", joint_target=0),
joint(joint_name="rh_LFJ5", joint_target=0),
joint(joint_name="rh_WRJ1", joint_target=0),
joint(joint_name="rh_WRJ2", joint_target=0)
]
# store step 2
store_2 = [
joint(joint_name="rh_THJ1", joint_target=50),
joint(joint_name="rh_THJ2", joint_target=13),
joint(joint_name="rh_THJ3", joint_target=11),
joint(joint_name="rh_THJ4", joint_target=65),
joint(joint_name="rh_THJ5", joint_target=27),
joint(joint_name="rh_FFJ0", joint_target=180),
joint(joint_name="rh_FFJ3", joint_target=90),
joint(joint_name="rh_FFJ4", joint_target=0),
joint(joint_name="rh_MFJ0", joint_target=180),
joint(joint_name="rh_MFJ3", joint_target=90),
joint(joint_name="rh_MFJ4", joint_target=0),
joint(joint_name="rh_RFJ0", joint_target=180),
joint(joint_name="rh_RFJ3", joint_target=90),
joint(joint_name="rh_RFJ4", joint_target=0),
joint(joint_name="rh_LFJ0", joint_target=180),
joint(joint_name="rh_LFJ3", joint_target=90),
joint(joint_name="rh_LFJ4", joint_target=0),
joint(joint_name="rh_LFJ5", joint_target=0),
joint(joint_name="rh_WRJ1", joint_target=0),
joint(joint_name="rh_WRJ2", joint_target=0)
]
# store step 3
store_3 = [
joint(joint_name="rh_THJ1", joint_target=0),
joint(joint_name="rh_THJ2", joint_target=0),
joint(joint_name="rh_THJ3", joint_target=0),
joint(joint_name="rh_THJ4", joint_target=65),
joint(joint_name="rh_THJ5", joint_target=0)
]
# business card pre-zero position
bc_pre_zero = [
joint(joint_name="rh_FFJ0", joint_target=13.6),
joint(joint_name="rh_FFJ3", joint_target=7),
joint(joint_name="rh_FFJ4", joint_target=-0.4),
joint(joint_name="rh_MFJ0", joint_target=51.1),
joint(joint_name="rh_MFJ3", joint_target=33),
joint(joint_name="rh_MFJ4", joint_target=21),
joint(joint_name="rh_RFJ0", joint_target=50),
joint(joint_name="rh_RFJ3", joint_target=18),
joint(joint_name="rh_RFJ4", joint_target=-21),
joint(joint_name="rh_LFJ0", joint_target=30),
joint(joint_name="rh_LFJ3", joint_target=0),
joint(joint_name="rh_LFJ4", joint_target=-24),
joint(joint_name="rh_LFJ5", joint_target=7),
joint(joint_name="rh_THJ1", joint_target=15.2),
joint(joint_name="rh_THJ2", joint_target=7.4),
joint(joint_name="rh_THJ3", joint_target=-4),
joint(joint_name="rh_THJ4", joint_target=50),
joint(joint_name="rh_THJ5", joint_target=-16.8)
]
# business card zero position
bc_zero = [
joint(joint_name="rh_FFJ0", joint_target=13.6),
joint(joint_name="rh_FFJ3", joint_target=7),
joint(joint_name="rh_FFJ4", joint_target=-0.4),
joint(joint_name="rh_MFJ0", joint_target=51.1),
joint(joint_name="rh_MFJ3", joint_target=32),
joint(joint_name="rh_MFJ4", joint_target=21),
joint(joint_name="rh_RFJ0", joint_target=50),
joint(joint_name="rh_RFJ3", joint_target=18),
joint(joint_name="rh_RFJ4", joint_target=-21),
joint(joint_name="rh_LFJ0", joint_target=30),
joint(joint_name="rh_LFJ3", joint_target=0),
joint(joint_name="rh_LFJ4", joint_target=-24),
joint(joint_name="rh_LFJ5", joint_target=7),
joint(joint_name="rh_THJ1", joint_target=17.2),
joint(joint_name="rh_THJ2", joint_target=10.4),
joint(joint_name="rh_THJ3", joint_target=-4),
joint(joint_name="rh_THJ4", joint_target=50),
joint(joint_name="rh_THJ5", joint_target=-13.6)
]
# business card position 1
bc_1 = [
joint(joint_name="rh_FFJ0", joint_target=137),
joint(joint_name="rh_FFJ3", joint_target=7)
]
# business card position 2 #UNUSED
bc_2 = [
joint(joint_name="rh_FFJ0", joint_target=137),
joint(joint_name="rh_FFJ3", joint_target=31)
]
# business card position 3
bc_3 = [
joint(joint_name="rh_FFJ0", joint_target=137),
joint(joint_name="rh_FFJ3", joint_target=58)
]
# business card position 4
bc_4 = [
joint(joint_name="rh_FFJ0", joint_target=66),
joint(joint_name="rh_FFJ3", joint_target=58)
]
# business card position 5
bc_5 = [
joint(joint_name="rh_FFJ0", joint_target=180),
joint(joint_name="rh_FFJ3", joint_target=58)
]
# business card position 6
bc_6 = [
joint(joint_name="rh_FFJ0", joint_target=180),
joint(joint_name="rh_FFJ3", joint_target=0)
]
# business card position 7
bc_7 = [
joint(joint_name="rh_FFJ0", joint_target=0),
joint(joint_name="rh_FFJ3", joint_target=0)
]
# business card position 8
bc_8 = [
joint(joint_name="rh_FFJ0", joint_target=137),
joint(joint_name="rh_FFJ3", joint_target=15)
]
# business card position 9 #UNUSED
bc_9 = [
joint(joint_name="rh_FFJ0", joint_target=137),
joint(joint_name="rh_FFJ3", joint_target=30)
]
# business card position 10 #UNUSED
bc_10 = [
joint(joint_name="rh_FFJ0", joint_target=137),
joint(joint_name="rh_FFJ3", joint_target=60)
]
# business card position 11 #UNUSED
bc_11 = [
joint(joint_name="rh_FFJ0", joint_target=137),
joint(joint_name="rh_FFJ3", joint_target=31)
]
# business card position 12
bc_12 = [
joint(joint_name="rh_FFJ0", joint_target=137),
joint(joint_name="rh_FFJ3", joint_target=58)
]
# business card position 13
bc_13 = [
joint(joint_name="rh_FFJ0", joint_target=66),
joint(joint_name="rh_FFJ3", joint_target=58)
]
# business card position 14
bc_14 = [
joint(joint_name="rh_MFJ3", joint_target=64),
joint(joint_name="rh_FFJ4", joint_target=20)
]
# business card position 15
bc_15 = [
joint(joint_name="rh_FFJ0", joint_target=81),
joint(joint_name="rh_FFJ4", joint_target=20),
joint(joint_name="rh_FFJ3", joint_target=50),
joint(joint_name="rh_THJ4", joint_target=55),
joint(joint_name="rh_THJ5", joint_target=20)
]
# business card position 16
bc_16 = [
joint(joint_name="rh_MFJ0", joint_target=20),
joint(joint_name="rh_MFJ3", joint_target=10),
joint(joint_name="rh_MFJ4", joint_target=0)
]
#A boolean used in this demo: set to true while an action is running
# just so we don't do 2 actions at once
action_running = mutex.mutex()
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)
#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 create_hand_publishers(self):
"""
Creates a dictionnary of publishers to send the targets to the controllers
on /sh_??j?_mixed_position_velocity_controller/command
"""
hand_pub = {}
for joint in [
"rh_FFJ0", "rh_FFJ3", "rh_FFJ4", "rh_MFJ0", "rh_MFJ3",
"rh_MFJ4", "rh_RFJ0", "rh_RFJ3", "rh_RFJ4", "rh_LFJ0",
"rh_LFJ3", "rh_LFJ4", "rh_LFJ5", "rh_THJ1", "rh_THJ2",
"rh_THJ3", "rh_THJ4", "rh_THJ5", "rh_WRJ1", "rh_WRJ2"
]:
hand_pub[joint] = rospy.Publisher('sh_' + joint.lower() +
'_position_controller/command',
Float64,
latch=True)
print 'sh_' + joint.lower() + '_position_controller/command'
return hand_pub
def hand_publish(self, pose):
"""
Publishes the given pose to the correct controllers for the hand.
The targets are converted in radians.
"""
for joint in pose:
print "joint", joint
print self.hand_publishers[joint.joint_name]
print math.radians(joint.joint_target)
self.hand_publishers[joint.joint_name].publish(
math.radians(joint.joint_target))
def callback_biotacs(self, msg):
"""
The callback function for the biotacs. Checks if one of the fingers
was pressed (filter the noise). If it is the case, call the
corresponding function.
@msg is the message containing the biotac data
"""
#loop through the five tactiles
for index, tactile in enumerate(msg.tactiles):
#here we're just checking pdc (the pressure)
# to see if a finger has been pressed, but you have
# access to the whole data from the sensor
# (look at sr_robot_msgs/msg/Biotac.msg)
if tactile.pdc >= PDC_THRESHOLD:
# the tactile has been pressed, call the
# corresponding function
self.fingers_pressed_functions[index](tactile.pdc)
def callback_psts(self, msg):
"""
The callback function for the PSTs. Checks if one of the fingers
was pressed (filter the noise). If it is the case, call the
corresponding function.
@msg is the message containing the biotac data
"""
#loop through the five tactiles
for index, tactile in enumerate(msg.pressure):
#here we're just checking the pressure
# to see if a finger has been pressed
# 18456 is the value the PST takes when the sensor is not plugged in
if tactile >= PST_THRESHOLD and tactile != 18456:
# the tactile has been pressed, call the
# corresponding function
self.fingers_pressed_functions[index](tactile)
def ff_pressed(self, data):
"""
The first finger was pressed.
@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')
rospy.loginfo("FF touched, running basic demo ")
#send the start position to the hand
self.hand_publish(self.start_pos_hand)
time.sleep(1)
self.hand_publish(self.flex_ff)
time.sleep(1)
self.hand_publish(self.ext_ff)
time.sleep(1)
self.hand_publish(self.flex_mf)
time.sleep(1)
self.hand_publish(self.ext_mf)
time.sleep(1)
self.hand_publish(self.flex_rf)
time.sleep(1)
self.hand_publish(self.ext_rf)
time.sleep(1)
self.hand_publish(self.flex_lf)
time.sleep(1)
self.hand_publish(self.ext_lf)
time.sleep(1)
self.hand_publish(self.flex_th_1)
time.sleep(1)
self.hand_publish(self.flex_th_2)
time.sleep(1)
self.hand_publish(self.ext_th_1)
time.sleep(1)
self.hand_publish(self.ext_th_2)
time.sleep(1)
self.hand_publish(self.n_wr)
time.sleep(1)
self.hand_publish(self.s_wr)
time.sleep(1)
self.hand_publish(self.zero_wr)
time.sleep(1)
self.hand_publish(self.e_wr)
time.sleep(1)
self.hand_publish(self.w_wr)
time.sleep(1)
self.hand_publish(self.zero_wr)
time.sleep(1)
self.hand_publish(self.pre_ff_ok)
time.sleep(0.3)
self.hand_publish(self.ff_ok)
time.sleep(1)
self.hand_publish(self.ff2mf_ok)
time.sleep(1)
self.hand_publish(self.mf_ok)
time.sleep(1)
self.hand_publish(self.mf2rf_ok)
time.sleep(1)
self.hand_publish(self.rf_ok)
time.sleep(1)
self.hand_publish(self.rf2lf_ok)
time.sleep(1)
self.hand_publish(self.lf_ok)
time.sleep(1)
self.hand_publish(self.start_pos_hand)
time.sleep(1)
self.hand_publish(self.flex_ff)
time.sleep(0.2)
self.hand_publish(self.flex_mf)
time.sleep(0.2)
self.hand_publish(self.flex_rf)
time.sleep(0.2)
self.hand_publish(self.flex_lf)
time.sleep(0.2)
self.hand_publish(self.ext_ff)
time.sleep(0.2)
self.hand_publish(self.ext_mf)
time.sleep(0.2)
self.hand_publish(self.ext_rf)
time.sleep(0.2)
self.hand_publish(self.ext_lf)
time.sleep(0.2)
self.hand_publish(self.flex_ff)
time.sleep(0.2)
self.hand_publish(self.flex_mf)
time.sleep(0.2)
self.hand_publish(self.flex_rf)
time.sleep(0.2)
self.hand_publish(self.flex_lf)
time.sleep(0.2)
self.hand_publish(self.ext_ff)
time.sleep(0.2)
self.hand_publish(self.ext_mf)
time.sleep(0.2)
self.hand_publish(self.ext_rf)
time.sleep(0.2)
self.hand_publish(self.ext_lf)
time.sleep(0.2)
self.hand_publish(self.flex_ff)
time.sleep(0.2)
self.hand_publish(self.flex_mf)
time.sleep(0.2)
self.hand_publish(self.flex_rf)
time.sleep(0.2)
self.hand_publish(self.flex_lf)
time.sleep(0.2)
self.hand_publish(self.ext_ff)
time.sleep(0.2)
self.hand_publish(self.ext_mf)
time.sleep(0.2)
self.hand_publish(self.ext_rf)
time.sleep(0.2)
self.hand_publish(self.ext_lf)
time.sleep(1.0)
self.hand_publish(self.pre_ff_ok)
time.sleep(0.3)
self.hand_publish(self.ff_ok)
time.sleep(1)
self.hand_publish(self.ne_wr)
time.sleep(1.2)
self.hand_publish(self.nw_wr)
time.sleep(1.2)
self.hand_publish(self.sw_wr)
time.sleep(1.2)
self.hand_publish(self.se_wr)
time.sleep(1.2)
self.hand_publish(self.ne_wr)
time.sleep(0.4)
self.hand_publish(self.nw_wr)
time.sleep(0.4)
self.hand_publish(self.sw_wr)
time.sleep(0.4)
self.hand_publish(self.se_wr)
time.sleep(0.4)
self.hand_publish(self.zero_wr)
time.sleep(1)
self.hand_publish(self.start_pos_hand)
time.sleep(1)
#wait before next possible action
time.sleep(.2)
self.action_running.unlock()
def mf_pressed(self, data):
"""
The middle finger was pressed.
@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 was pressed
#p = subprocess.Popen('beep')
rospy.loginfo("MF touched, running business card demo ")
#wait 1s for the user to release the sensor
time.sleep(.2)
print "self.store_3", self.store_3
#send the start position to the hand
self.hand_publish(self.store_3)
time.sleep(1)
self.hand_publish(self.start_pos_hand)
time.sleep(1)
self.hand_publish(self.bc_pre_zero)
time.sleep(2)
self.hand_publish(self.bc_zero)
time.sleep(3)
self.hand_publish(self.bc_1)
time.sleep(1)
# self.hand_publish( self.bc_2 )
# time.sleep(1)
self.hand_publish(self.bc_3)
time.sleep(1)
self.hand_publish(self.bc_4)
time.sleep(1)
self.hand_publish(self.bc_5)
time.sleep(1)
self.hand_publish(self.bc_6)
time.sleep(1)
self.hand_publish(self.bc_7)
time.sleep(1)
self.hand_publish(self.bc_8)
time.sleep(1)
# self.hand_publish( self.bc_9 )
# time.sleep(1)
# self.hand_publish( self.bc_10 )
# time.sleep(1)
# self.hand_publish( self.bc_11 )
# time.sleep(1)
self.hand_publish(self.bc_12)
time.sleep(1)
self.hand_publish(self.bc_13)
time.sleep(1)
# self.hand_publish( self.bc_14 )
# time.sleep(1)
self.hand_publish(self.bc_15)
time.sleep(1)
self.hand_publish(self.bc_16)
time.sleep(3)
self.hand_publish(self.start_pos_hand)
#wait before next possible action
time.sleep(3)
self.action_running.unlock()
def rf_pressed(self, data):
"""
The ring finger was pressed.
@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 was pressed
#p = subprocess.Popen('beep')
rospy.loginfo("RF touched, running shaking hands demo.")
#wait 1s for the user to release the sensor
time.sleep(.2)
#send the start position to the hand
self.hand_publish(self.shake_grasp_1)
time.sleep(3)
self.hand_publish(self.shake_grasp_2)
time.sleep(1)
self.hand_publish(self.e_wr)
time.sleep(0.33)
self.hand_publish(self.w_wr)
time.sleep(0.33)
self.hand_publish(self.zero_wr)
time.sleep(0.66)
self.hand_publish(self.shake_grasp_1)
time.sleep(3)
self.hand_publish(self.start_pos_hand)
time.sleep(2)
#wait before next possible action
time.sleep(.2)
self.action_running.unlock()
def lf_pressed(self, data):
"""
The little finger was pressed.
@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 store 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)
time.sleep(1)
self.hand_publish(self.flex_lf)
time.sleep(0.2)
self.hand_publish(self.flex_rf)
time.sleep(0.2)
self.hand_publish(self.flex_mf)
time.sleep(0.2)
self.hand_publish(self.flex_ff)
time.sleep(0.2)
self.hand_publish(self.ext_lf)
time.sleep(0.2)
self.hand_publish(self.ext_rf)
time.sleep(0.2)
self.hand_publish(self.ext_mf)
time.sleep(0.2)
self.hand_publish(self.ext_ff)
time.sleep(0.2)
self.hand_publish(self.flex_lf)
time.sleep(0.2)
self.hand_publish(self.flex_rf)
time.sleep(0.2)
self.hand_publish(self.flex_mf)
time.sleep(0.2)
self.hand_publish(self.flex_ff)
time.sleep(1)
self.hand_publish(self.store_1)
time.sleep(1)
self.hand_publish(self.store_2)
time.sleep(1)
#wait before next possible action
time.sleep(.2)
self.action_running.unlock()
def th_pressed(self, data):
"""
The thumb was pressed.
@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 was pressed
#p = subprocess.Popen('beep')
rospy.loginfo("TH touched, going to start position.")
#wait 1s for the user to release the sensor
time.sleep(.2)
#send the thumb_up_position to the hand
self.hand_publish(self.start_pos_hand)
#wait before next possible action
time.sleep(.2)
self.action_running.unlock()
class FancyDemo(object):
# type of controller that is running
controller_type = "_position_controller"
# starting position for the hand (DON't use until reviewed. Should be executed in two movement sequences)
start_pos_hand = { "THJ1":21,
"THJ2":25,
"THJ3":0,
"THJ4":50,
"THJ5":-6,
"FFJ0":180,
"FFJ3":90,
"FFJ4":0,
"MFJ0":180,
"MFJ3":90,
"MFJ4":0,
"RFJ0":180,
"RFJ3":90,
"RFJ4":0,
"LFJ0":180,
"LFJ3":90,
"LFJ4":0,
"LFJ5":0,
"WRJ1":0,
"WRJ2":0 }
# starting position for the hand
extended_pos_hand = { "THJ1":0,
"THJ2":0,
"THJ3":0,
"THJ4":0,
"THJ5":-6,
"FFJ0":0,
"FFJ3":0,
"FFJ4":0,
"MFJ0":0,
"MFJ3":0,
"MFJ4":0,
"RFJ0":0,
"RFJ3":0,
"RFJ4":0,
"LFJ0":0,
"LFJ3":0,
"LFJ4":0,
"LFJ5":0,
"WRJ1":0,
"WRJ2":0 }
#thumb up position for the hand
thumb_up_pos_hand = { "THJ1":0,
"THJ2":0,
"THJ3":0,
"THJ4":0,
"THJ5":0,
"FFJ0":180,
"FFJ3":90,
"FFJ4":0,
"MFJ0":180,
"MFJ3":90,
"MFJ4":0,
"RFJ0":180,
"RFJ3":90,
"RFJ4":0,
"LFJ0":180,
"LFJ3":90,
"LFJ4":0,
"LFJ5":0,
"WRJ1":0,
"WRJ2":10 }
#starting position for the arm
start_pos_arm = [ joint(joint_name = "ElbowJRotate", joint_target = 0),
joint(joint_name = "ElbowJSwing", joint_target = 59),
joint(joint_name = "ShoulderJRotate", joint_target = 0),
joint(joint_name = "ShoulderJSwing", joint_target = 33)
]
#The arm publisher:
# publish the message to the /sr_arm/sendupdate topic.
arm_publisher = rospy.Publisher('/sr_arm/sendupdate', sendupdate)
#A boolean used in this demo: set to true while an action is running
# just so we don't do 2 actions at once
action_running = mutex.mutex()
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 create_hand_publishers(self):
"""
Creates a dictionnary of publishers to send the targets to the controllers
on /sh_??j?_position_controller/command
"""
hand_pub = {}
for joint in ["FFJ0", "FFJ3", "FFJ4",
"MFJ0", "MFJ3", "MFJ4",
"RFJ0", "RFJ3", "RFJ4",
"LFJ0", "LFJ3", "LFJ4", "LFJ5",
"THJ1", "THJ2", "THJ3", "THJ4", "THJ5",
"WRJ1", "WRJ2" ]:
hand_pub[joint] = rospy.Publisher('sh_'+joint.lower() + self.controller_type + '/command', Float64, latch=True)
return hand_pub
def hand_publish(self, pose):
"""
Publishes the given pose to the correct controllers for the hand.
The targets are converted in radians.
"""
for joint, pos in pose.iteritems():
self.hand_publishers[joint].publish( math.radians(pos) )
def callback_biotacs(self, msg):
"""
The callback function for the biotacs. Checks if one of the fingers
was pressed (filter the noise). If it is the case, call the
corresponding function.
@msg is the message containing the biotac data
"""
#loop through the five tactiles
for index,tactile in enumerate(msg.tactiles):
#here we're just checking pdc (the pressure)
# to see if a finger has been pressed, but you have
# access to the whole data from the sensor
# (look at sr_robot_msgs/msg/Biotac.msg)
if tactile.pdc >= PDC_THRESHOLD:
# the tactile has been pressed, call the
# corresponding function
self.fingers_pressed_functions[index](tactile.pdc)
def callback_psts(self, msg):
"""
The callback function for the PSTs. Checks if one of the fingers
was pressed (filter the noise). If it is the case, call the
corresponding function.
@msg is the message containing the biotac data
"""
#loop through the five tactiles
for index,tactile in enumerate(msg.pressure):
#here we're just checking the pressure
# to see if a finger has been pressed
# 18456 is the value the PST takes when the sensor is not plugged in
if tactile >= PST_THRESHOLD and tactile != 18456:
# the tactile has been pressed, call the
# corresponding function
self.fingers_pressed_functions[index](tactile)
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 mf_pressed(self, data):
"""
The middle finger was pressed.
If no action is currently running, we move the arm angles
to a positive 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 finger 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 * (20.0)
target_elb_swing = 90. + data * (20.0)
target_elb_rot = 0. + 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 rf_pressed(self, data):
"""
The ring finger was pressed.
If no action is currently running, we make a beep
but don't do anything else.
@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 was pressed
p = subprocess.Popen('beep')
rospy.loginfo("RF touched, not doing anything.")
#wait before next possible action
time.sleep(.2)
self.action_running.unlock()
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 th_pressed(self, data):
"""
The thumb was pressed.
If no action is currently running, we send the thumb_up
targets to the hand.
@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 was pressed
p = subprocess.Popen('beep')
rospy.loginfo("TH touched, going to thumb up position.")
#wait 1s for the user to release the sensor
time.sleep(.2)
#send the thumb_up_position to the hand
self.hand_publish( self.thumb_up_pos_hand )
#wait before next possible action
time.sleep(.2)
self.action_running.unlock()
#
# This program is distributed in the hope that it will be useful, but WITHOUT
# ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
# FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
# more details.
#
# You should have received a copy of the GNU General Public License along
# with this program. If not, see <http://www.gnu.org/licenses/>.
#
from sr_robot_msgs.msg import joint
# start position for counting: punch
punch_step1 = [
joint(joint_name="THJ1", joint_target=68),
joint(joint_name="THJ2", joint_target=30),
joint(joint_name="THJ3", joint_target=15),
joint(joint_name="THJ4", joint_target=70),
joint(joint_name="THJ5", joint_target=30),
joint(joint_name="FFJ0", joint_target=0),
joint(joint_name="FFJ3", joint_target=0),
joint(joint_name="FFJ4", joint_target=0),
joint(joint_name="MFJ0", joint_target=0),
joint(joint_name="MFJ3", joint_target=0),
joint(joint_name="MFJ4", joint_target=0),
joint(joint_name="RFJ0", joint_target=0),
joint(joint_name="RFJ3", joint_target=0),
joint(joint_name="RFJ4", joint_target=0),
joint(joint_name="LFJ0", joint_target=0),
joint(joint_name="LFJ3", joint_target=0),
