def set_new_target(target):
state = YuMiState()
state.joint1 = target[0]
state.joint2 = target[1]
state.joint7 = target[2]
state.joint3 = target[3]
state.joint4 = target[4]
state.joint5 = target[5]
state.joint6 = target[6]
return state
def main():
global home_left
global tool_cesar_cal
rospy.init_node('tcp_tf', anonymous=True)
br = tf2_ros.TransformBroadcaster()
y = YuMiRobot(arm_type='remote')
listener = tf.TransformListener()
y.left.set_tool(tool_cesar_cal)
rate = rospy.Rate(10)
move(y)
y.left.open_gripper(no_wait=False, wait_for_res=True)
pose_state = y.left.get_pose(raw_res=False)
y.left.close_gripper(no_wait=False, wait_for_res=True)
state_robot = False
temp_flag = False
previous_trans = [0.0, 0.0, 0.0]
while (not rospy.is_shutdown()):
print("flag:{}".format(temp_flag))
try:
(trans, rot) = listener.lookupTransform('/world', '/pose_object',
rospy.Time(0))
if not trans == None and not rot == None and not previous_trans == trans:
pose_state.translation = trans
temp_flag = y.left.is_pose_reachable(pose_state)
move(y)
previous_trans = trans
print("It is feasible to reach the given pose {}".format(
temp_flag))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
if temp_flag:
state_robot = y.left.goto_pose(pose_state,
linear=True,
relative=False,
wait_for_res=True)
y.left.open_gripper(no_wait=False, wait_for_res=True)
pose_state.translation[2] = 0.065
state_robot = y.left.goto_pose(pose_state,
linear=True,
relative=False,
wait_for_res=True)
y.left.close_gripper(force=20, no_wait=False, wait_for_res=True)
move(y)
y.left.goto_state(YuMiState(home_industrial_obj),
wait_for_res=False)
y.left.open_gripper(no_wait=False, wait_for_res=True)
temp_flag = False
print('moving!!!')
rate.sleep()
def move(yumi_robot):
import time
global g_index_last_move
global g_timestamp_last_move
#Object that encapsulates a yumi arm joint angle configuration.
moves = [YuMiState(p) for p in moves_deg]
yumi_robot.left.goto_state(moves[0], wait_for_res=False)
yumi_robot.right.goto_state(moves[1], wait_for_res=False)
g_timestamp_last_move = time.time()
def move(yumi_robot):
import time
global g_index_last_move
global g_timestamp_last_move
if (time.time() - g_timestamp_last_move) < 3:
return
#Object that encapsulates a yumi arm joint angle configuration.
moves = [YuMiState(p) for p in moves_deg]
g_index_last_move = (g_index_last_move + 1) % len(moves)
yumi_robot.left.goto_state(moves[g_index_last_move], wait_for_res=False)
g_timestamp_last_move = time.time()
def main():
global home_left
global tool_cesar_cal
rospy.init_node('tcp_tf', anonymous=True)
br = tf2_ros.TransformBroadcaster()
y = YuMiRobot(arm_type='remote')
listener = tf.TransformListener()
y.left.set_tool(tool_cesar_cal)
y.right.set_tool(tool_cesar_cal)
rate = rospy.Rate(10)
move(y)
y.left.close_gripper(no_wait=False, wait_for_res=True)
y.right.close_gripper(no_wait=False, wait_for_res=True)
state_robot = False
temp_flag_left = False
temp_flag_right = False
temp_flag = False
previous_trans = [0.0, 0.0, 0.0]
pose_state_left = y.left.get_pose(raw_res=False)
pose_state_right = y.right.get_pose(raw_res=False)
y.left.open_gripper(no_wait=False, wait_for_res=True)
y.right.open_gripper(no_wait=False, wait_for_res=True)
#exit(0)
while (not rospy.is_shutdown()):
print("temp_flag_left:{}".format(temp_flag_left))
print("temp_flag_right:{}".format(temp_flag_right))
try:
(trans, rot) = listener.lookupTransform('/world', '/pose_object',
rospy.Time(0))
print('rot:{}'.format(rot))
print('trans:{}'.format(trans))
#roll_, pitch_, yaw_=tf.transformations.euler_from_quaternion(rot)
#print(yaw_)
if not trans == None and not rot == None and not previous_trans == trans:
if trans[1] < -0.01:
print('right {}'.format(trans[1]))
pose_state_right.translation = trans
temp_flag_right = True
temp_flag_left = False
else:
if trans[1] > 0.01:
print('left {}'.format(trans[1]))
pose_state_left.translation = trans
temp_flag_right = False
temp_flag_left = True
move(y)
previous_trans = trans
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
if temp_flag_left:
y.left.goto_pose(pose_state_left,
linear=True,
relative=False,
wait_for_res=True)
pose_state_left.translation[2] = 0.065
#pose_state_right.translation[1]+=0.065
y.left.goto_pose(pose_state_left,
linear=True,
relative=False,
wait_for_res=True)
y.left.close_gripper(force=20, no_wait=False, wait_for_res=True)
move(y)
y.left.goto_state(YuMiState(state_left_h), wait_for_res=False)
y.left.open_gripper(no_wait=False, wait_for_res=True)
temp_flag_left = False
else:
if temp_flag_right:
y.right.goto_pose(pose_state_right,
linear=True,
relative=False,
wait_for_res=True)
pose_state_right.translation[2] = 0.065
#pose_state_right.translation[1]+=0.065
y.right.goto_pose(pose_state_right,
linear=True,
relative=False,
wait_for_res=True)
y.right.close_gripper(force=20,
no_wait=False,
wait_for_res=True)
move(y)
y.right.goto_state(YuMiState(state_right_h),
wait_for_res=False)
y.right.open_gripper(no_wait=False, wait_for_res=True)
temp_flag_right = False
print('moving!!!')
rate.sleep()
def main():
global home_left
global tool_cesar_cal
rospy.init_node('tcp_tf', anonymous=True)
br = tf2_ros.TransformBroadcaster()
y = YuMiRobot(arm_type='remote')
listener = tf.TransformListener()
y.left.set_tool(tool_cesar_cal)
rate = rospy.Rate(10)
move(y)
y.left.open_gripper(no_wait=False, wait_for_res=True)
pose_state = y.left.get_pose(raw_res=False)
y.left.close_gripper(no_wait=False, wait_for_res=True)
state_robot=False
temp_flag=False
previous_trans=[0.0 , 0.0, 0.0]
#trick
tmp=pose_state.quaternion
#print('tmp',tmp)
orientation_list = [tmp[1],tmp[2],tmp[3],tmp[0]]
(roll_default, pitch_default, yaw_default)=tf.transformations.euler_from_quaternion(orientation_list)
#print("euler_angles:{}".format([math.degrees(roll_default),math.degrees(pitch_default),math.degrees(yaw_default)]))
while (not rospy.is_shutdown()):
print("flag:{}".format(temp_flag))
try:
(trans,rot) = listener.lookupTransform('/world','/pose_object', rospy.Time(0))
from tf.transformations import euler_from_quaternion, quaternion_from_euler
# ########### TEST#####################
# print('pose_state.quaternion:{}'.format(pose_state.quaternion))
# print('pose_state.rotation:{}'.format(pose_state.rotation))
# tmp=pose_state.quaternion
# print('tmp',tmp)
# orientation_list = [tmp[1],tmp[2],tmp[3],tmp[0]]
# (roll_default, pitch_default, yaw_default)=tf.transformations.euler_from_quaternion(orientation_list)
# print("euler_angles:{}".format([math.degrees(roll_default),math.degrees(pitch_default),math.degrees(yaw_default)]))
roll_, pitch_, yaw_=tf.transformations.euler_from_quaternion(rot)
#print('cesar:{}{}{}'.format(roll_, pitch_, yaw_))
# euler_angles=[math.degrees(roll_),math.degrees(pitch_),math.degrees(yaw_)]
# print('euler_angles:{}'.format(euler_angles))
print('---New computation about quaternions---')
q_orig = tf.transformations.quaternion_from_euler(roll_default,pitch_default,yaw_default)
q_rot = quaternion_from_euler(0, 0, yaw_)
q_new = quaternion_multiply(q_rot, q_orig)
q_new/=np.linalg.norm(q_new)
r_new = quaternion_matrix(q_new)
# print('r_new:',r_new)
# # print(type(r_new))
# # print(r_new[:-1,:-1])
# # pose_state.quaternion[0]=q_new[3]
# # pose_state.quaternion[1]=q_new[0]
# # pose_state.quaternion[2]=q_new[1]
# # pose_state.quaternion[3]=q_new[2]
# pose_state.translation=trans
# pose_state.rotation=r_new[:-1,:-1]
# state_robot=y.left.goto_pose(pose_state, linear=True, relative=False, wait_for_res=True)
# print('state_robot:{}'.format(state_robot))
# exit(0)
################END TEST########################
if not trans==None and not rot==None and not previous_trans==trans:
pose_state.translation=trans
pose_state.rotation=r_new[:-1,:-1]
temp_flag=y.left.is_pose_reachable(pose_state)
move(y)
previous_trans=trans
print("It is feasible to reach the given pose {}".format(temp_flag))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
if temp_flag:
state_robot=y.left.goto_pose(pose_state, linear=True, relative=False, wait_for_res=True)
y.left.open_gripper(no_wait=False, wait_for_res=True)
pose_state.translation[2]=0.065
state_robot=y.left.goto_pose(pose_state, linear=True, relative=False, wait_for_res=True)
y.left.close_gripper(force=20,no_wait=False, wait_for_res=True)
move(y)
y.left.goto_state(YuMiState(home_industrial_obj),wait_for_res=False)
y.left.open_gripper(no_wait=False, wait_for_res=True)
temp_flag=False
print('moving!!!')
rate.sleep()
def manipulation(yc, mode):
global g_camera_base
global trajectory_pose_b
global trajectory_pose_c
global left_pose, right_pose
global trajectory_list_b
global trajectory_list_c
global qr_pose_c
yc.go_threading_home('left')
yc.get_hand('left').close_gripper()
yc.get_hand('left').goto_state(YuMiState([-76.67, -72.86, 22.74, 92.88, -32.53, 125.14, 87.63]))
calibration_singal.set()
###################### MODE SLECTION #############################
# SELECT THE MODE HERE
MODE = mode
###################### MODE SLECTION #############################
K = [1, 1, 1]
K = [i * 0.2 for i in K]
limit = 0.004 # meters
tolerance = 0.005 # meters
# Mode 1: follow a planned trajectory in base frame
if MODE == 1:
# raw_input('Stable the camera and press to start!!!')
time.sleep(2)
seg_points = 10
trajectory_list_b = []
tolerance = 0.005 # 5mm
left_pose_trans = left_pose.get(True)[:3, -1]
if trajectory_pose_b is None or left_pose_trans is None:
print('No data recognized!')
target_trans = trajectory_pose_b[:3, -1]
print('target_trans is :', target_trans)
# vector substract between target point (yellow point) in camera frame and left end-effector pose
v = [((x1 - x2)/ seg_points).tolist() for (x1, x2) in zip(target_trans, left_pose_trans)]
v = [v[0][0][0], v[1][0][0], v[2][0][0]]
# add the segmeted point into trajectory
for p in range(1, seg_points + 2):
tmp = map(operator.add, left_pose_trans, [i * p for i in v])
trajectory_list_b.append(tmp)
for index in range(len(trajectory_list_b)):
print(index)
print(trajectory_list_b[index])
# follow the current trajectory point
while True:
left_pose_translation = left_pose.get(True)
e = [(x1 - x2).tolist() for (x1, x2) in zip(trajectory_list_b[index], left_pose_translation[:3, -1])]
if all(np.abs(e) < tolerance):
index +=1
break
u = [i * j for i, j in zip(K, e)]
if np.abs(e[0]) < tolerance:
u[0] = 0
if np.abs(e[1]) < tolerance:
u[1] = 0
if np.abs(e[2]) < tolerance:
u[2] = 0
for index_e, value_e in enumerate(e):
if u[index_e] > limit:
u[index_e] = limit
elif u[index_e] < -limit:
u[index_e] = -limit
yc.move_delta('left', trans=u, rotation=None)
# print(e)
# print(u)
print '------------ Finished --------------------'
# Mode 2: real-time tracking
if MODE == 2:
while True:
left_pose_translation = left_pose.get(True)[:3, -1]
if trajectory_pose_b is None or left_pose_translation is None:
continue
e = [(x1 - x2).tolist() for (x1, x2) in zip(trajectory_pose_b[:3, -1], left_pose_translation)]
e = [e[0][0][0], e[1][0][0], e[2][0][0]]
if all(np.abs(e) < tolerance):
continue
u = [i * j for i, j in zip(K, e)]
if np.abs(e[0]) < tolerance:
u[0] = 0
if np.abs(e[1]) < tolerance:
u[1] = 0
if np.abs(e[2]) < tolerance:
u[2] = 0
for index_e, value_e in enumerate(e):
if u[index_e] > limit:
u[index_e] = limit
elif u[index_e] < -limit:
u[index_e] = -limit
yc.move_delta('left', trans=u, rotation=None)
# print ('-----------', u, e)
# Mode 3: planned trajectory purely using the camera frame
if MODE == 3:
time.sleep(3)
seg_points = 10
trajectory_list_c = []
qr_pose = qr_pose_c[-1]
if qr_pose is None:
print('No data recognized!')
v = [((x1 - x2) / seg_points).tolist() for (x1, x2) in zip(trajectory_pose_c[:3, -1], qr_pose[:3, -1])]
v = [v[0][0][0], v[1][0][0], v[2][0][0]]
for p in range(1, seg_points + 1):
tmp = map(operator.add, qr_pose[:3, -1], [i * p for i in v])
trajectory_list_c.append(tmp)
for index in range(len(trajectory_list_c)):
print(index)
print(trajectory_list_c[index])
while True:
qr_translation = qr_pose_c[-1][:3, -1].tolist()
qr_translation = [qr_translation[0][0], qr_translation[1][0], qr_translation[2][0]]
cur_target = [trajectory_list_c[index][0][0][0,0], trajectory_list_c[index][1][0][0,0], trajectory_list_c[index][2][0][0,0]]
e_c = [(x1 - x2).tolist() * 0.01 for (x1, x2) in zip(cur_target, qr_translation)]
if all(np.abs(e_c) < tolerance):
break
K2 = [1, 1, 1]
K2 = [i * -0.1 for i in K2]
u = [i * j for i, j in zip(K2, e_c)]
for index_e, value_e in enumerate(e_c):
if u[index_e] > limit:
u[index_e] = limit
elif u[index_e] < -limit:
u[index_e] = -limit
yc.move_delta('left', trans=u, rotation=None)
print('+', u, e_c)
import numpy as np
import time
from yumipy import YuMiRobot, YuMiState
from autolab_core import RigidTransform
if __name__ == '__main__':
num_attempts = 3
state = YuMiState([51.16, -99.4, 21.57, -107.19, 84.11, 94.61, -36.00])
robot = YuMiRobot(debug=False)
robot.set_v(50)
for i in range(num_attempts):
print 'Trying collision', i
robot.right.goto_state(state)
robot.right.goto_pose_delta([0, 0.236, 0])
robot.right.goto_pose_delta([0.053, 0, 0])
robot.right.goto_pose_delta([0, 0, -0.145])
time.sleep(3)
def main():
global home_left
rospy.init_node('tcp_tf', anonymous=True)
br = tf2_ros.TransformBroadcaster()
y = YuMiRobot(arm_type='remote')
global tool_cesar_cal
y.left.set_tool(tool_cesar_cal)
y.right.set_tool(tool_cesar_cal)
state_left = y.left.get_state(raw_res=False)
state_right = y.right.get_state(raw_res=False)
print('state_left: {}'.format(state_left))
#print('state_right: {}'.format(state_right))
exit(0)
y.left.close_gripper(no_wait=False, wait_for_res=True)
y.right.close_gripper(no_wait=False, wait_for_res=True)
y.left.open_gripper(no_wait=False, wait_for_res=True)
y.right.open_gripper(no_wait=False, wait_for_res=True)
exit(0)
#bridge position---------------------
state_left_b = [-90.8, -59.74, 17.98, 112.48, 87.42, -61.76, 66.21]
state_right_b = [92.48, -55.08, 15.49, -111.06, 82.59, 55.73, -70.21]
y.left.goto_state(YuMiState(state_left_b), wait_for_res=False)
y.right.goto_state(YuMiState(state_right_b), wait_for_res=False)
#home position-----------------------
state_left_h = [-59.49, -77.18, 16.56, 112.06, 63.68, -112.18, 66.82]
state_right_h = [59.78, -72.42, 21.53, -112.84, 63.46, -55.3, -67.8]
y.left.goto_state(YuMiState(state_left_h), wait_for_res=False)
y.right.goto_state(YuMiState(state_right_h), wait_for_res=False)
exit(0)
rate = rospy.Rate(10)
while (True):
pose = y.left.get_pose(raw_res=False)
print('pose: {}'.format(pose))
state = y.left.get_state(raw_res=False)
print("state: {}".format(state))
print('moving!!!')
# pose.translation=[0.48, 0.10, 0.04]
# #pose.translation[1] += 0.03
# #print(y.left.is_pose_reachable(pose))
# state_robot_1=y.left.goto_pose(pose, linear=True, relative=False, wait_for_res=True)
# print('state_robot {}'.format(state_robot_1))
# if state_robot_1:
# #y.left.open_gripper(no_wait=False, wait_for_res=True)
# time.sleep(3)
# print('done...')
# pose.translation=[0.45, 0.05, 0.03]
# #print(y.left.is_pose_reachable(pose))
# state_robot_2=y.left.goto_pose(pose, linear=True, relative=False, wait_for_res=True)
# #y.left.close_gripper(no_wait=False, wait_for_res=True)
# time.sleep(3)
# if(state_robot_2):
# print('done...')
# pose.translation=[0.50, 0.10, 0.03]
# #print(y.left.is_pose_reachable(pose))
# state_robot_3=y.left.goto_pose(pose, linear=True, relative=False, wait_for_res=True)
# time.sleep(3)
# continue
# else:
# pose.translation[0]=3
# move(y)
rate.sleep()
class YumiConstants:
T_gripper_gripperV = rt.RigidTransform(rotation=[[-1, 0, 0], [0, 1, 0],
[0, 0, -1]],
from_frame='gripper',
to_frame='obj')
T_rightH_yumi_1 = rt.RigidTransform(
rotation=[[0, 0, 1], [1, 0, 0], [0, 1, 0]],
translation=[0.6256, -0.15060002, 0.3616],
from_frame='home',
to_frame='yumi')
T_rightH_yumi_2 = rt.RigidTransform(
rotation=[[0, 0, 1], [1, 0, 0], [0, 1, 0]],
translation=[0.6256 - 0.1, -0.15060002 + 0.1, 0.3616],
from_frame='home',
to_frame='yumi')
T_rightH_yumi_3 = rt.RigidTransform(
rotation=[[0, 0, 1], [1, 0, 0], [0, 1, 0]],
translation=[0.6256 - 0.1, -0.15060002 + 0.1, 0.3616 - 0.05],
from_frame='home',
to_frame='yumi')
T_leftH_yumi_1 = rt.RigidTransform(
rotation=[[1, 0, 0], [0, 0, -1], [0, 1, 0]],
translation=[0.52070004, 0.07340001, 0.3574],
from_frame='home',
to_frame='yumi')
T_leftH_yumi_2 = rt.RigidTransform(
rotation=[[1, 0, 0], [0, 0, -1], [0, 1, 0]],
translation=[0.67080003 - 0.15, -0.12650001 + 0.2, 0.35720003],
from_frame='home',
to_frame='yumi')
T_board_yumi = rt.RigidTransform(translation=[0.3984, 0, 0.0837],
from_frame='board',
to_frame='yumi')
board_center = rt.RigidTransform(rotation=[[-1, 0, 0], [0, 1, 0],
[0, 0, -1]],
translation=[0.42971, -0.004, -0.057],
from_frame='yumi',
to_frame='world')
T_rightH_yumi = rt.RigidTransform(
rotation=[[-1, 0, 0], [0, 1, 0], [0, 0, -1]],
translation=[0.3984, 0 - 8 * 0.0375, 0.0837],
from_frame='home',
to_frame='yumi')
T_leftH_yumi = rt.RigidTransform(
rotation=[[-1, 0, 0], [0, 1, 0], [0, 0, -1]],
translation=[0.3984, 0 + 8 * 0.0375, 0.0837],
# translation=[0.3984, 0 + 8*0.0375, 0.0837],
from_frame='home',
to_frame='yumi')
right_threading_home = YuMiState(
[101.34, -83.3, 54.01, -44.34, -82.32, -26.22, -76.76])
left_threading_home = YuMiState(
[-74.73, -70.63, 9.62, 15.86, 65.74, -169.18, 50.61])
right_pickup_home = YuMiState(
[80.92, -118.47, 39.2, -139.35, 107.91, 4.83, -26.93])
left_pickup_home = YuMiState(
[-75.32, -114.45, 37.59, 134.52, 102.66, -8.73, 42.77])
"""
Helper script to move YuMi back to home pose
Author: Jeff Mahler
"""
from yumipy import YuMiRobot, YuMiState
from yumipy import YuMiConstants as YMC
if __name__ == '__main__':
y = YuMiRobot()
y.set_z('fine')
y.set_v(200)
y.left.close_gripper()
y.right.close_gripper()
state = YuMiState([69.42, -75.37, 20.21, -110.77, 91.29, -62.35, -43.74])
#y.left.goto_pose(YMC.L_HOME_POSE)
y.right.goto_state(state)
#y.stop()
#exit(0)
for i, tup in enumerate(YMC.BOX_CLOSE_SEQUENCE):
print i
arm, box_state = tup
if arm == 'L':
if isinstance(box_state, YuMiState):
y.left.goto_state(box_state)
elif isinstance(box_state, list):
y.left.goto_pose_delta(box_state)
else:
y.left.goto_pose(box_state)
