class GraspExecution(object):
def __init__(self):
self.keyboard_pressed = False
self.hand_commander = SrHandCommander(name='right_hand')
self.grasp_yaml = {}
def _open_yaml(self):
grasp_config_filename = '/home/user/projects/shadow_robot/base/src/'\
'sr_interface/sr_example/config/demo_grasps.yaml'
with open(grasp_config_filename) as f:
self.grasp_yaml = yaml.load(f, Loader=yaml.FullLoader)
def _get_input(self):
fd = sys.stdin.fileno()
old_settings = termios.tcgetattr(fd)
try:
tty.setraw(sys.stdin.fileno())
ch = sys.stdin.read(1)
finally:
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
return ch
def run(self):
self._open_yaml()
while True:
input_val = self._get_input()
if input_val == "1":
self.execute_grasp("open_hand")
elif input_val == "2":
self.execute_grasp("close_hand")
elif input_val == "3":
self.execute_grasp("point")
elif input_val == "4":
self.execute_grasp("2_finger_pinch")
elif input_val == "5":
self.execute_grasp("3_finger_pinch")
elif input_val == "6":
self.execute_grasp("parallel_extension")
elif input_val == "7":
self.execute_grasp("grasp_sphere")
if '0x1b' == hex(ord(input_val)):
sys.exit(0)
def execute_grasp(self, grasp):
rospy.loginfo("Grasp {} started.".format(grasp))
open_dict = dict(
zip(self.grasp_yaml['joint_names'],
self.grasp_yaml['grasps']['open_hand']))
grasp_dict = dict(
zip(self.grasp_yaml['joint_names'],
self.grasp_yaml['grasps'][grasp]))
self.hand_commander.move_to_joint_value_target_unsafe(
open_dict, 5.0, True)
self.hand_commander.move_to_joint_value_target_unsafe(
grasp_dict, 5.0, True)
rospy.sleep(2.0)
rospy.loginfo("Grasp {} completed.".format(grasp))
'la_wrist_1_joint': -0.89,
'la_wrist_2_joint': -0.92,
'la_shoulder_lift_joint': -1.93,
'la_wrist_3_joint': 0.71,
'lh_WRJ1': 0.0,
'lh_WRJ2': 0.0
}
# Move through each goal
# joint states are sent to the commanders, with a time for execution and a flag as to whether
# or not the commander should wait for the command to complete before moving to the next command.
# Move hand
joint_goals = hand_joints_goal_1
rospy.loginfo("Moving hand to joint states\n" + str(joint_goals) + "\n")
hand_commander.move_to_joint_value_target_unsafe(joint_goals, 1.0, False)
# Move arm
joint_goals = arm_joints_goal_1
rospy.loginfo("Moving arm to joint states\n" + str(joint_goals) + "\n")
arm_commander.move_to_joint_value_target_unsafe(joint_goals, 3.0, True)
# Move hand
joint_goals = hand_joints_goal_2
rospy.loginfo("Moving hand to joint states\n" + str(joint_goals) + "\n")
hand_commander.move_to_joint_value_target_unsafe(joint_goals, 3.0, True)
# Move hand
joint_goals = hand_joints_goal_3
rospy.loginfo("Moving hand to joint states\n" + str(joint_goals) + "\n")
hand_commander.move_to_joint_value_target_unsafe(joint_goals, 3.0, False)
0.05, 1.04, 0.34, 0.68, -0.24, 0.35,
0.69, 0.18, 1.20, -0.11]
grasp_pose = dict(zip(keys, position))
# Adjust poses according to the hand loaded
open_hand_current = dict([(i, open_hand[i]) for i in joints if i in open_hand])
grasp_pose_current = dict([(i, grasp_pose[i]) for i in joints if i in grasp_pose])
# Partial list of goals
grasp_partial_1 = {'rh_FFJ3': 0.50}
start_time = rospy.Time.now()
# Move hand using move_to_joint_value_target_unsafe to 1st position
hand_commander.move_to_joint_value_target_unsafe(open_hand_current, 1.0, True)
rospy.sleep(2)
# Move hand using run_joint_trajectory_unsafe to joint angles specified in 'position' list
hand_commander.move_to_joint_value_target_unsafe(grasp_pose_current, 1.0, False)
trajectory_start_time = 2.0
joint_trajectory = JointTrajectory()
# Construct and send partial trajectory for joint listed in grasp_partial_1
joint_trajectory.header.stamp = start_time + rospy.Duration.from_sec(float(trajectory_start_time))
joint_trajectory.joint_names = list(grasp_partial_1.keys())
joint_trajectory.points = []
trajectory_point = construct_trajectory_point(grasp_partial_1, 1.0)
joint_trajectory.points.append(trajectory_point)
'ra_wrist_2_joint': -1.595231835042135,
}
# =============== Main ===============
if __name__ == '__main__':
ff = BiotacData()
ff_elect_db = ff._elect_db
pub_angle = rospy.Publisher("ff_angle", Float32, queue_size=10)
pub_pac1 = rospy.Publisher("ff_pac1", Float32, queue_size=10)
pub_pdc = rospy.Publisher("ff_pdc", Float32, queue_size=10)
# Move arm and hand to start position
joint_goals = arm_start
arm_commander.move_to_joint_value_target_unsafe(joint_goals, 5, True)
joint_goals = hand_start
hand_commander.move_to_joint_value_target_unsafe(joint_goals, 5, True)
# ?? Check for object here using Biotacs ??
while True: # find pdc baseline
print("Establishing Pdc baseline...")
rospy.Subscriber("rh/tactile", BiotacAll, pdc_callback)
rate_handle.sleep()
if len(ff._pdc_db) >= 50:
# print(ff._pdc_db)
break
# Move arm to pickup location
joint_goals = arm_pickup
arm_commander.move_to_joint_value_target_unsafe(joint_goals, 5, True)
# Grab the object with the hand
joint_goals = hand_close
y = np.multiply(y,settings_gain)
y = y + settings_ymin
return(y)
# Sigmoid Symmetric Transfer Function
def tansig_apply(n):
a = 2 / (1 + np.exp(-2*n)) -1
return(a)
# Map Minimum and Maximum Output Reverse-Processing Function
def mapminmax_reverse(y,settings_gain,settings_xoffset,settings_ymin):
x = y - settings_ymin
x = x / settings_gain
x = x + settings_xoffset
return(x)
#---------------------------------------------------------------------------------
def listen(): rospy.Subscriber("/rh/tactile/", BiotacAll, callback4) rospy.spin()
# hand_commander.move_to_joint_value_target_unsafe(start, 3, True)
# hand_commander.move_to_joint_value_target_unsafe(close, 3, True)
# time.sleep(2)
if __name__ == '__main__':
hand_commander.move_to_joint_value_target_unsafe(start, 1, True)
hand_commander.move_to_joint_value_target_unsafe(close, 2, True)
slip = Slip()
time.sleep(1)
listen()
'lh_WRJ1': 0.0, 'lh_WRJ2': 0.0}
pack_hand_1 = {'lh_FFJ1': 1.5707, 'lh_FFJ2': 1.5707, 'lh_FFJ3': 1.5707, 'lh_FFJ4': 0.0,
'lh_MFJ1': 1.5707, 'lh_MFJ2': 1.5707, 'lh_MFJ3': 1.5707, 'lh_MFJ4': 0.0,
'lh_RFJ1': 1.5707, 'lh_RFJ2': 1.5707, 'lh_RFJ3': 1.5707, 'lh_RFJ4': 0.0,
'lh_LFJ1': 1.5707, 'lh_LFJ2': 1.5707, 'lh_LFJ3': 1.5707, 'lh_LFJ4': 0.0, 'lh_LFJ5': 0.0}
pack_hand_2 = {'lh_THJ4': 1.2}
pack_hand_3 = {'lh_THJ1': 0.52, 'lh_THJ2': 0.61, 'lh_THJ5': 0.43}
# Move hand to open position
joint_states = open_hand
rospy.loginfo("Moving hand to open position")
hand_commander.move_to_joint_value_target_unsafe(joint_states, 2.0, False)
rospy.sleep(2)
# Move hand to closed position
joint_states = pack_hand_1
rospy.loginfo("Moving hand to pack position")
hand_commander.move_to_joint_value_target_unsafe(joint_states, 2.0, False)
rospy.sleep(2)
joint_states = pack_hand_2
hand_commander.move_to_joint_value_target_unsafe(joint_states, 2.0, False)
rospy.sleep(2)
joint_states = pack_hand_3
hand_commander.move_to_joint_value_target_unsafe(joint_states, 2.0, False)
rospy.sleep(2)
class Teleoperation():
def __init__(self):
self.bridge = CvBridge()
self.hand_commander = SrHandCommander(name="right_hand")
rospy.sleep(1)
def online_once(self):
while True:
img_data = rospy.wait_for_message(
"/camera/aligned_depth_to_color/image_raw", Image)
rospy.loginfo("Got an image ^_^")
try:
img = self.bridge.imgmsg_to_cv2(img_data,
desired_encoding="passthrough")
except CvBridgeError as e:
rospy.logerr(e)
try:
# preproces
img = seg_hand_depth(img, 500, 1000, 10, 100, 4, 4, 250, True,
300)
img = img.astype(np.float32)
img = img / 255. * 2. - 1
n = cv2.resize(img, (0, 0), fx=2, fy=2)
n = cv2.normalize(n,
n,
alpha=0,
beta=1,
norm_type=cv2.NORM_MINMAX,
dtype=cv2.CV_32F)
cv2.imshow("segmented human hand", n)
cv2.waitKey(1)
# get the clipped joints
goal = tuple(self.joint_cal(img, isbio=True))
hand_pos = self.hand_commander.get_joints_position()
# first finger
hand_pos.update({"rh_FFJ3": goal[3]})
hand_pos.update({"rh_FFJ2": goal[4]})
hand_pos.update({"rh_FFJ4": goal[2]})
# middle finger
hand_pos.update({"rh_MFJ3": goal[12]})
hand_pos.update({"rh_MFJ2": goal[13]})
# ring finger
hand_pos.update({"rh_RFJ3": goal[16]})
hand_pos.update({"rh_RFJ2": goal[17]})
# little finger
hand_pos.update({"rh_LFJ3": goal[8]})
hand_pos.update({"rh_LFJ2": goal[9]})
# thumb
hand_pos.update({"rh_THJ5": goal[19]})
hand_pos.update({"rh_THJ4": goal[20]})
hand_pos.update({"rh_THJ3": goal[21]})
hand_pos.update({"rh_THJ2": goal[22]})
self.hand_commander.move_to_joint_value_target_unsafe(
hand_pos, 0.3, False, angle_degrees=False)
#rospy.sleep(0.5)
rospy.loginfo("Next one please ---->")
except:
rospy.loginfo("no images")
def joint_cal(self, img, isbio=False):
# start = rospy.Time.now().to_sec()
# run the model
feature = test(model, img)
# network_time = rospy.Time.now().to_sec() - start
# print("network_time is ", network_time)
joint = [0.0, 0.0]
joint += feature.tolist()
if isbio:
joint[5] = 0.3498509706185152
joint[10] = 0.3498509706185152
joint[14] = 0.3498509706185152
joint[18] = 0.3498509706185152
joint[23] = 0.3498509706185152
# joints crop
joint[2] = self.clip(joint[2], 0.349, -0.349)
joint[3] = self.clip(joint[3], 1.57, 0)
joint[4] = self.clip(joint[4], 1.57, 0)
joint[5] = self.clip(joint[5], 1.57, 0)
joint[6] = self.clip(joint[6], 0.785, 0)
joint[7] = self.clip(joint[7], 0.349, -0.349)
joint[8] = self.clip(joint[8], 1.57, 0)
joint[9] = self.clip(joint[9], 1.57, 0)
joint[10] = self.clip(joint[10], 1.57, 0)
joint[11] = self.clip(joint[11], 0.349, -0.349)
joint[12] = self.clip(joint[12], 1.57, 0)
joint[13] = self.clip(joint[13], 1.57, 0)
joint[14] = self.clip(joint[14], 1.57, 0)
joint[15] = self.clip(joint[15], 0.349, -0.349)
joint[16] = self.clip(joint[16], 1.57, 0)
joint[17] = self.clip(joint[17], 1.57, 0)
joint[18] = self.clip(joint[18], 1.57, 0)
joint[19] = self.clip(joint[19], 1.047, -1.047)
joint[20] = self.clip(joint[20], 1.222, 0)
joint[21] = self.clip(joint[21], 0.209, -0.209)
joint[22] = self.clip(joint[22], 0.524, -0.524)
joint[23] = self.clip(joint[23], 1.57, 0)
return joint
def clip(self, x, maxv=None, minv=None):
if maxv is not None and x > maxv:
x = maxv
if minv is not None and x < minv:
x = minv
return x
'lh_LFJ1': 0.01406673264624339,
'lh_LFJ4': -0.22573803931632768,
'lh_LFJ5': 0.7547867069581923,
'lh_RFJ4': -0.1442490766383741,
'lh_RFJ1': 0.03331992208352813,
'lh_RFJ2': 1.2615629035756921,
'lh_RFJ3': 0.158866717571428,
'lh_MFJ1': 0.03215803503674591,
'lh_MFJ3': 0.14292484557904367,
'lh_MFJ2': 0.9464753316391405,
'lh_MFJ4': 0.023358296483200973,
'lh_WRJ2': -0.05471159545568462,
'lh_WRJ1': -0.011152559328240493
}
hand_commander.move_to_joint_value_target_unsafe(open_hand, 2.0, False)
rospy.sleep(2)
hand_commander.move_to_joint_value_target_unsafe(FF_Flex, 0.75, False)
rospy.sleep(0.75)
hand_commander.move_to_joint_value_target_unsafe(FF_Ext, 0.75, False)
rospy.sleep(0.75)
hand_commander.move_to_joint_value_target_unsafe(MF_Flex, 0.75, False)
rospy.sleep(0.75)
hand_commander.move_to_joint_value_target_unsafe(MF_Ext, 0.75, False)
rospy.sleep(0.75)
hand_commander.move_to_joint_value_target_unsafe(RF_Flex, 0.75, False)
rospy.sleep(0.75)
hand_commander.move_to_joint_value_target_unsafe(RF_Ext, 0.75, False)
rospy.sleep(0.75)
hand_commander.move_to_joint_value_target_unsafe(LF_Flex, 0.75, False)
rospy.sleep(0.75)
'joint_1': 0.00
},
'state_2': {
'joint_0': 0.00,
'joint_1': 0.00
}
}
"""
rospy.init_node("use_exported_states")
# Define a hand commander, so we have something to do with the states we extracted.
hand_commander = SrHandCommander()
# You could use the states directly:
hand_commander.move_to_joint_value_target_unsafe(warehouse_states['state_2'])
rospy.sleep(2)
hand_commander.move_to_joint_value_target_unsafe(warehouse_states['state_1'])
rospy.sleep(2)
hand_commander.move_to_joint_value_target_unsafe(warehouse_states['state_2'])
rospy.sleep(2)
# You could translate all the named states in a trajectory:
trajectory = [
{
'name': 'state_1',
'interpolate_time': 3.0
},
{
'name': 'state_2',
'interpolate_time': 3.0,
class Teleoperation():
def __init__(self):
self.bridge = CvBridge()
self.hand_commander = SrHandCommander(name="right_hand")
# play piano start pos
start_play_pos = {
"rh_THJ1": 20,
"rh_THJ2": 10,
"rh_THJ3": 0,
"rh_THJ4": 0,
"rh_THJ5": 0,
"rh_FFJ1": 45,
"rh_FFJ2": 80,
"rh_FFJ3": 0,
"rh_FFJ4": 0,
"rh_MFJ1": 45,
"rh_MFJ2": 80,
"rh_MFJ3": 0,
"rh_MFJ4": 0,
"rh_RFJ1": 45,
"rh_RFJ2": 80,
"rh_RFJ3": 0,
"rh_RFJ4": 0,
"rh_LFJ1": 45,
"rh_LFJ2": 80,
"rh_LFJ3": 0,
"rh_LFJ4": 0,
"rh_LFJ5": 0,
"rh_WRJ1": -30,
"rh_WRJ2": 0
}
self.hand_commander.move_to_joint_value_target_unsafe(
start_play_pos, 1.5, False, angle_degrees=True)
rospy.sleep(1)
def online_once(self):
while True:
# img_data = rospy.wait_for_message("/camera/depth/image_raw", Image)
img_data = rospy.wait_for_message(
"/camera/aligned_depth_to_color/image_raw", Image)
rospy.loginfo("Got an image ^_^")
try:
img = self.bridge.imgmsg_to_cv2(img_data,
desired_encoding="passthrough")
except CvBridgeError as e:
rospy.logerr(e)
try:
# preproces
img = seg_hand_depth(img, 500, 1000, 10, 100, 4, 4, 250, True,
300)
img = img.astype(np.float32)
img = img / 255. * 2. - 1
print(img.shape)
n = cv2.resize(img, (0, 0), fx=2, fy=2)
n1 = cv2.normalize(n,
n,
alpha=0,
beta=1,
norm_type=cv2.NORM_MINMAX,
dtype=cv2.CV_32F)
cv2.imshow("segmented human hand", n1)
cv2.waitKey(1)
# get the clipped joints
goal = tuple(self.joint_cal(img, isbio=True))
# bug:get_joints_position() return radian joints
hand_pos = self.hand_commander.get_joints_position()
# first finger
hand_pos.update({"rh_FFJ3": goal[3]})
#hand_pos.update({"rh_FFJ2": goal[4]})
# hand_pos.update({"rh_FFJ4": goal[2]})
# middle finger
hand_pos.update({"rh_MFJ3": goal[12]})
#hand_pos.update({"rh_MFJ2": goal[13]})
# ring finger
hand_pos.update({"rh_RFJ3": goal[16]})
#hand_pos.update({"rh_RFJ2": goal[17]})
# little finger
hand_pos.update({"rh_LFJ3": goal[8]})
#hand_pos.update({"rh_LFJ2": goal[9]})
# thumb
#hand_pos.update({"rh_THJ5": goal[19]})
#hand_pos.update({"rh_THJ4": goal[20]})
#hand_pos.update({"rh_THJ3": goal[21]})
#hand_pos.update({"rh_THJ2": goal[22]})
self.hand_commander.move_to_joint_value_target_unsafe(
hand_pos, 0.3, False, angle_degrees=False)
#rospy.sleep(0.5)
# collision check and manipulate
# csl_client = rospy.ServiceProxy('CheckSelfCollision', checkSelfCollision)
# try:
# shadow_pos = csl_client(start, goal)
# if shadow_pos.result:
# rospy.loginfo("Move Done!")
# else:
# rospy.logwarn("Failed to move!")
# except rospy.ServiceException as exc:
# rospy.logwarn("Service did not process request: " + str(exc))
rospy.loginfo("Next one please ---->")
except:
rospy.loginfo("no images")
def joint_cal(self, img, isbio=False):
# start = rospy.Time.now().to_sec()
# run the model
feature = test(model, img)
# network_time = rospy.Time.now().to_sec() - start
# print("network_time is ", network_time)
joint = [0.0, 0.0]
joint += feature.tolist()
if isbio:
joint[5] = 0.3498509706185152
joint[10] = 0.3498509706185152
joint[14] = 0.3498509706185152
joint[18] = 0.3498509706185152
joint[23] = 0.3498509706185152
# joints crop
joint[2] = self.clip(joint[2], 0.349, -0.349)
joint[3] = self.clip(joint[3], 1.57, 0)
joint[4] = self.clip(joint[4], 1.57, 0)
joint[5] = self.clip(joint[5], 1.57, 0)
joint[6] = self.clip(joint[6], 0.785, 0)
joint[7] = self.clip(joint[7], 0.349, -0.349)
joint[8] = self.clip(joint[8], 1.57, 0)
joint[9] = self.clip(joint[9], 1.57, 0)
joint[10] = self.clip(joint[10], 1.57, 0)
joint[11] = self.clip(joint[11], 0.349, -0.349)
joint[12] = self.clip(joint[12], 1.57, 0)
joint[13] = self.clip(joint[13], 1.57, 0)
joint[14] = self.clip(joint[14], 1.57, 0)
joint[15] = self.clip(joint[15], 0.349, -0.349)
joint[16] = self.clip(joint[16], 1.57, 0)
joint[17] = self.clip(joint[17], 1.57, 0)
joint[18] = self.clip(joint[18], 1.57, 0)
joint[19] = self.clip(joint[19], 1.047, -1.047)
joint[20] = self.clip(joint[20], 1.222, 0)
joint[21] = self.clip(joint[21], 0.209, -0.209)
joint[22] = self.clip(joint[22], 0.524, -0.524)
joint[23] = self.clip(joint[23], 1.57, 0)
return joint
def clip(self, x, maxv=None, minv=None):
if maxv is not None and x > maxv:
x = maxv
if minv is not None and x < minv:
x = minv
return x
"lh_WRJ1": 0, "lh_WRJ2": 0}
# store step 2 Bio_Tac
store_2_BioTac = {"lh_THJ1": 20, "lh_THJ2": 36, "lh_THJ3": 0, "lh_THJ4": 30, "lh_THJ5": -3,
"lh_FFJ1": 90, "lh_FFJ2": 90, "lh_FFJ3": 90, "lh_FFJ4": 0,
"lh_MFJ1": 90, "lh_MFJ2": 90, "lh_MFJ3": 90, "lh_MFJ4": 0,
"lh_RFJ1": 90, "lh_RFJ2": 90, "lh_RFJ3": 90, "lh_RFJ4": 0,
"lh_LFJ1": 90, "lh_LFJ2": 90, "lh_LFJ3": 90, "lh_LFJ4": 0, "lh_LFJ5": 0,
"lh_WRJ1": 0, "lh_WRJ2": 0}
# store step 3
store_3 = {"lh_THJ1": 0, "lh_THJ2": 0, "lh_THJ3": 0, "lh_THJ4": 65, "lh_THJ5": 0}
for x in range(0, 100):
print "We're on iteration number %d" % (x)
rospy.sleep(1)
hand_commander.move_to_joint_value_target_unsafe(store_3, 1.1, False, angle_degrees=True)
rospy.sleep(1.1)
hand_commander.move_to_joint_value_target_unsafe(start_pos, 1.1, False, angle_degrees=True)
rospy.sleep(1.1)
hand_commander.move_to_joint_value_target_unsafe(flex_ff, 1.0, False, angle_degrees=True)
rospy.sleep(1.1)
hand_commander.move_to_joint_value_target_unsafe(ext_ff, 1.0, False, angle_degrees=True)
rospy.sleep(1.1)
hand_commander.move_to_joint_value_target_unsafe(flex_mf, 1.0, False, angle_degrees=True)
rospy.sleep(1.1)
hand_commander.move_to_joint_value_target_unsafe(ext_mf, 1.0, False, angle_degrees=True)
rospy.sleep(1.1)
hand_commander.move_to_joint_value_target_unsafe(flex_rf, 1.0, False, angle_degrees=True)
rospy.sleep(1.1)
hand_commander.move_to_joint_value_target_unsafe(ext_rf, 1.0, False, angle_degrees=True)
rospy.sleep(1.1)
# --------- This is the divide between TemplatePot1 and TemplatePot2 ------------
# User input ends here::::
# ==============================================================================
hand_joint_names = ['rh_FFJ1','rh_FFJ2', 'rh_FFJ3', 'rh_FFJ4','rh_MFJ1','rh_MFJ2','rh_MFJ3','rh_MFJ4','rh_RFJ1','rh_RFJ2','rh_RFJ3','rh_RFJ4','rh_LFJ1','rh_LFJ2','rh_LFJ3','rh_LFJ4','rh_LFJ5','rh_THJ1','rh_THJ2','rh_THJ3','rh_THJ4','rh_THJ5','rh_WRJ1','rh_WRJ2'];
arm_joint_names = ['ra_shoulder_pan_joint', 'ra_shoulder_lift_joint', 'ra_elbow_joint', 'ra_wrist_1_joint', 'ra_wrist_2_joint', 'ra_wrist_3_joint'];
move_dict_arm = {};
move_dict_hand = {};
#move hand to start position
rospy.loginfo("Move to hand_start position")
hand_commander.move_to_joint_value_target_unsafe(hand_start, 4, True)
arm_commander.move_to_joint_value_target_unsafe(arm_start, 4, True)
rospy.sleep(1.0)
#run loop at 50Hz
r = rospy.Rate(50)
while not rospy.is_shutdown():
for i in range(len(movements_Arm)):
current_move_dict_arm = movements_Arm[i];
current_move_dict_hand = movements_Hand[i];
tlist = times[i];
print('Next movement: ' + movements[i])
