def spoof_rhr_data():
pub = rospy.Publisher('/reflex_takktile2/hand_state', Hand, queue_size=10)
rospy.init_node('spoof_reflex_hand', anonymous=True)
r = rospy.Rate(50)
hand = Hand()
counter = 0.0
cycle_period = 40.0
while not rospy.is_shutdown():
sine_signal = sin(counter / cycle_period)
finger_angle = (1.4 * sine_signal) + 1.4
preshape_angle = (-(pi / 4) * sine_signal) + pi / 4
if (sine_signal > 0):
contact = True
else:
contact = False
scalar = (40 * sine_signal) + 40
for i in range(3):
hand.finger[i].proximal = finger_angle # Proximal joints
hand.finger[i].distal_approx = 0.25 * finger_angle # Distal joints
for j in range(9):
hand.finger[i].contact[j] = contact # Finger tactile contact
hand.finger[i].pressure[j] = scalar # Finger pressure scalar
hand.motor[3].joint_angle = preshape_angle
pub.publish(hand)
counter += 1
r.sleep()
def spoof_rhr_data():
pub = rospy.Publisher('/reflex_hand', Hand, queue_size=10)
rospy.init_node('spoof_reflex_hand', anonymous=True)
r = rospy.Rate(50)
hand = Hand()
counter = 0.0
rate_of_cycle = 40.0
while not rospy.is_shutdown():
sine = sin(counter/rate_of_cycle)
fing_ang = (1.4*sine) + 1.4
pre_ang = (-(pi/4)*sine)+pi/4
if (sine > 0):
contact = True
else:
contact = False
scalar = (200*sine) + 200
for i in range(3):
hand.finger[i].proximal = fing_ang # Proximal joints
hand.finger[i].distal = 0.5*fing_ang # Distal joints
for j in range(9):
hand.finger[i].contact[j] = contact # Finger tactile contact
hand.finger[i].pressure[j] = scalar # Finger pressure scalar
hand.palm.preshape = pre_ang
for i in range(6):
hand.palm.contact[i] = contact
hand.palm.pressure[i] = scalar
pub.publish(hand)
counter+=1
r.sleep()
def __init__ (self):
super(ReFlex, self).__init__()
# motion parameters
self.FINGER_STEP = 0.05 # radians / 0.01 second
self.FINGER_STEP_LARGE = 0.15 # radians / 0.01 second
# Mabel added, for more speedy guarded_move for fast data collection
self.FINGER_STEP_HUGE = 0.35 # radians / 0.01 second. ~20 degrees
self.TENDON_MIN = 0.0 # max opening (radians)
#self.TENDON_MAX = 4.0 # max closure (radians)
# Mabel changed based on empirical. More than 3.14 penetrates palm!
self.TENDON_MAX = 2.8 # max closure (radians)
self.PRESHAPE_MIN = 0.0 # max opening (radians)
# Copied to preshape_# joint <limit> tag in
# urdf/full_reflex_model.urdf.xacro
self.PRESHAPE_MAX = 1.6 # max closure (radians)
self.hand = Hand ()
topic = '/reflex_hand'
rospy.loginfo('ReFlex class is subscribing to topic %s', topic)
rospy.Subscriber(topic, Hand, self.__hand_callback)
# Example code for a script using the ReFlex Takktile hand
# Note: you must connect a hand by running "roslaunch reflex reflex.launch" before you can run this script
from math import pi, cos
import rospy
from std_srvs.srv import Empty
from reflex_msgs.msg import Command
from reflex_msgs.msg import PoseCommand
from reflex_msgs.msg import VelocityCommand
from reflex_msgs.msg import ForceCommand
from reflex_msgs.msg import Hand
hand_state = Hand()
def main():
##################################################################################################################
rospy.init_node('ExampleHandNode')
# Services can automatically call hand calibration
calibrate_fingers = rospy.ServiceProxy('/reflex_plus/calibrate_fingers',
Empty)
calibrate_tactile = rospy.ServiceProxy('/reflex_plus/calibrate_tactile',
Empty)
# Services can set tactile thresholds and enable tactile stops
# enable_tactile_stops = rospy.ServiceProxy('/reflex_takktile2/enable_tactile_stops', Empty)
# disable_tactile_stops = rospy.ServiceProxy('/reflex_takktile2/disable_tactile_stops', Empty)
def handCB(self, msg):
self.sensor_values = Hand(msg.finger, msg.palm, msg.joints_publishing,
msg.tactile_publishing)
def publish_reflex_hand_state (self):
# If not initialized yet, that means no contacts yet. Just init all to 0s
if not self.contacts:
self.reset_contacts ()
sim_hand_msg = Hand ()
# Currently not used
sim_hand_msg.palm = Palm ()
#####
# Joint data
#####
# Populate sim_hand_msg, by packaging all contacts since the last time this
# fn was called (caller should enforce calling this fn only every
# 1/HERTZ seconds, so can publish /reflex_hand at HERTZ rate).
# I don't have joints data right now. Not sure how to get it from Gazebo
# yet, probably can through ros controller or something
# This is not too important for sim, because Gazebo sim automatically
# publishes tf, which is all I use. Hardware needs this, because otherwise
# software has no way of knowing.
sim_hand_msg.joints_publishing = True
success, preshape_1_pos = self.get_joint_pos ('preshape_1')
success, proximal_1_pos = self.get_joint_pos ('proximal_joint_1')
success, distal_1_pos = self.get_joint_pos ('distal_joint_1')
# Don't need preshape2, it should be enforced as negative of preshape1,
# `.` on real hand, there is only one joint.
success, proximal_2_pos = self.get_joint_pos ('proximal_joint_2')
success, distal_2_pos = self.get_joint_pos ('distal_joint_2')
success, proximal_3_pos = self.get_joint_pos ('proximal_joint_3')
success, distal_3_pos = self.get_joint_pos ('distal_joint_3')
# Populate msg
if preshape_1_pos:
sim_hand_msg.palm.preshape = preshape_1_pos [0]
if proximal_1_pos:
sim_hand_msg.finger[0].proximal = proximal_1_pos [0]
if distal_1_pos:
sim_hand_msg.finger[0].distal = distal_1_pos [0]
if proximal_2_pos:
sim_hand_msg.finger[1].proximal = proximal_2_pos [0]
if distal_2_pos:
sim_hand_msg.finger[1].distal = distal_2_pos [0]
if proximal_3_pos:
sim_hand_msg.finger[2].proximal = proximal_3_pos [0]
if distal_3_pos:
sim_hand_msg.finger[2].distal = distal_3_pos [0]
#####
# Tactile data
#####
# True to indicate tactile values are accurate. For simulation, it's always
# accurate, `.` no zeroing is required.
sim_hand_msg.tactile_publishing = True
for i in range (0, len (self.contacts)):
for j in range (0, len (self.contacts [i])):
# If got a contact from this sensor, set to true, else false.
if self.contacts [i] [j]:
#sim_hand_msg.finger [i].contact.append (True)
sim_hand_msg.finger [i].contact [j] = True
sim_hand_msg.finger [i].pressure [j] = self.PRESSED
#print ('Finger %d sensor %d set to True' % (i+1, j+1))
else:
#sim_hand_msg.finger [i].contact.append (False)
sim_hand_msg.finger [i].contact [j] = False
sim_hand_msg.finger [i].pressure [j] = 0.0
# Debug: print first /reflex_hand msg
#print (sim_hand_msg)
#####
# Publish msg
#####
# Publish 10 times, `.` UDP drops packages
for i in range (0, 10):
self.sim_hand_pub.publish (sim_hand_msg)
#print ('Published /reflex_hand msg')
# Restart a time segment, after having published this one
self.restart_timer = True
def __init__(self):
super(ReFlex, self).__init__()
# basic commands
self.BASE_FINGER_COMMANDS = [
'goto', 'guarded_move', 'guarded_move_fast', 'solid_contact',
'solid_fingertips_contact', 'avoid_contact', 'maintain_contact',
'dither', 'hold'
]
self.FINGER_MAP = {'f1': 0, 'f2': 1, 'f3': 2}
# motion parameters
self.FINGER_STEP = 0.05 # radians / 0.01 second
self.FINGER_STEP_LARGE = 0.15 # radians / 0.01 second
self.TENDON_MIN = 0.0 # max opening (radians)
self.TENDON_MAX = 4.0 # max closure (radians)
self.PRESHAPE_MIN = 0.0 # max opening (radians)
self.PRESHAPE_MAX = 1.6 # max closure (radians)
# control loop logic parameters for safety checking
self.ARRIVAL_ERROR = 0.05 # error at which controller considers
# position control finished
self.BLOCKED_ERROR = 0.05 # if the finger hasn't moved this far
# in hand_hist steps, it's blocked
self.BLOCKED_TIME = 0.3 # seconds before checking for blockage
# it's necessary to wait, otherwise the
# finger will interpret the time spenthttp://answers.ros.org/question/197109/how-does-mavros-rc-override-work/?answer=200242#post-id-200242
# still before movement as blocked
# Initialize logic variables
self.control_mode = ['goto', 'goto',
'goto'] # control mode for each finger
self.working = [
False, False, False
] # completion criteria (service returns when all false)
self.call_time = [-1.0, -1.0, -1.0,
-1.0] # used to track when commands are called
self.cmd_spool = np.array(
[-1.0, -1.0,
-1.0]) # finger commanded position, radians spool rotation
self.cmd_spool_old = deepcopy(
self.cmd_spool
) # Previous cmd position. If current cmd position matches, give no command
self.target_spool = np.array([0.0, 0.0, 0.0])
# Set up publishers
self.actuator_pub = rospy.Publisher('/set_reflex_hand',
RadianServoPositions,
queue_size=1)
# Subscribe to sensor information
self.hand_publishing = True
self.hand = Hand()
self.hand_hist = [
] # history of incoming hand data, bounded in __hand_callback
self.HISTORY_LENGTH = 15 # how many snapshots of old data to keep for checking motor stall, etc
self.SUBSCRIBE_TIME = 5 # time (seconds) that code will wait for a subscription
self.state_debugger = StateDebugger()
topic = '/reflex_hand'
rospy.loginfo('ReFlex class is subscribing to topic %s', topic)
rospy.Subscriber(topic, Hand, self.__hand_callback)
time_waiting = 0.0
while len(self.hand_hist) < 2\
and time_waiting < self.SUBSCRIBE_TIME\
and not rospy.is_shutdown():
rospy.sleep(0.01)
time_waiting += 0.01
self.hand_publishing = did_subscribe_succeed(time_waiting, topic)
if not self.hand_publishing:
rospy.logwarn(
'\tCheck that your hand is plugged in to power and the ethernet port'
)
rospy.loginfo('Reflex class is initialized')