def main():
with tf.Session() as sess:
sess.run(tf.global_variables_initializer())
def dynamics(x,t,tau_v,g_v,q0_v,grasping_point,sess):
position = grasping_point
s_v = canoSystem(tau_v,t)
feeddict = {g[0]:g_v[0],g[1]:g_v[1],g[2]:g_v[2],g[3]:g_v[3],g[4]:g_v[4],g[5]:g_v[5],
q[0]:x[0],q[1]:x[1],q[2]:x[2],q[3]:x[3],q[4]:x[4],q[5]:x[5],
qd[0]:x[6],qd[1]:x[7],qd[2]:x[8],qd[3]:x[9],qd[4]:x[10],qd[5]:x[11],
q0[0]:q0_v[0],q0[1]:q0_v[1],q0[2]:q0_v[2],q0[3]:q0_v[3],q0[4]:q0_v[4],q0[5]:q0_v[5],
tau:tau_v,s:s_v,xg:position[0],yg:position[1],zg:position[2]
}
qdd1_v,qdd2_v,qdd3_v,qdd4_v,qdd5_v,qdd6_v = sess.run(dmpNet,feed_dict = feeddict)
dx = [x[6],x[7],x[8],x[9],x[10],x[11],qdd1_v,qdd2_v,qdd3_v,qdd4_v,qdd5_v,qdd6_v]
return dx
period = 50
t = np.linspace(2, period, 100)
print(t)
tau_v = float(1)/period
# q0_v = [-0.0003235975848596695, -1.040771786366598, 1.6213598251342773, -0.34193402925600225, 1.5711277723312378, 3.141711950302124]
q0_v = [0, -pi/2, pi/2, 0, pi/2, pi]
v0 = [0,0,0,0,0,0]
x0 = []; x0.extend(q0_v); x0.extend(v0)
print("Initiate Object Detection")
STATE = 'NONE'
_,_,_, last_capture = IntelRealsense.pingpong_detection(display = False)
T_camera2base = IntelRealsense.transformation_camera2base()
T_end2base = IntelRealsense.transformation_end2base()
gripper = np.array([ [0],
[0],
[0.1],
[1] ])
Y = np.array([ [0],
[0],
[0],
[0],
[0],
[0] ])
X = np.array([ [np.random.normal(0, sigma_x)],
[np.random.normal(0, sigma_y)],
[np.random.normal(0, sigma_d)],
[np.random.normal(0, sigma_v_x)],
[np.random.normal(0, sigma_v_y)],
[np.random.normal(0, sigma_v_d)] ])
X, P = init_state(X, Y, 0)
print('First State is %s' %X)
I = np.identity(6, dtype=float)
H = np.identity(6, dtype=float)
count = 0
global client
try:
rospy.init_node("dmp", anonymous=True, disable_signals=True)
client = actionlib.SimpleActionClient('follow_joint_trajectory', FollowJointTrajectoryAction)
client.wait_for_server()
print ("Connected to server")
parameters = rospy.get_param(None)
index = str(parameters).find('prefix')
if (index > 0):
prefix = str(parameters)[index+len("prefix': '"):(index+len("prefix': '")+str(parameters)[index+len("prefix': '"):-1].find("'"))]
for i, name in enumerate(JOINT_NAMES):
JOINT_NAMES[i] = prefix + name
book = Workbook()
sheet = book.active
excel_row = 1
while(True):
initial_time = time.time()
pp_x, pp_y, pp_depth, capture = IntelRealsense.pingpong_detection(display = True)
pingpong_camera = IntelRealsense.reverse_perspective_projection(pp_x, pp_y, pp_depth); pingpong_camera[3] = 1
processing = capture - last_capture; timestep = processing
pingpong_base = T_camera2base.dot(pingpong_camera)
v_x, v_depth, v_z, a_x, a_y, a_depth, STATE = IntelRealsense.pingpong_velocity(STATE, pingpong_base[0,0], pingpong_base[1,0], pingpong_base[2,0], capture, display = False)
last_capture = capture
sheet.cell(row = excel_row,column = 1).value = pingpong_base[0]
sheet.cell(row = excel_row,column = 2).value = pingpong_base[1]
sheet.cell(row = excel_row,column = 3).value = pingpong_base[2]
sheet.cell(row = excel_row,column = 4).value = v_x
sheet.cell(row = excel_row,column = 5).value = v_depth
sheet.cell(row = excel_row,column = 6).value = v_z
sheet.cell(row = excel_row,column = 7).value = a_x
sheet.cell(row = excel_row,column = 8).value = a_y
sheet.cell(row = excel_row,column = 9).value = a_depth
excel_row +=1
if excel_row == 10000:
book.save('/home/s/catkin_ws/src/camera/src/real_world_variance.xlsx')
print('Excel Saved')
# print('Real World Coordinates From KF \n %s \n' %pingpong_camera)
# print('Ping-Pong Position (Base Frame) \n %s \n' %pingpong_base)
grasping_point = [pingpong_base[0], pingpong_base[1], pingpong_base[2]]
# print('grasping point %s' %grasping_point)
pingpong_base = pingpong_base - T_end2base.dot(gripper)
inp_invKine = IntelRealsense.transformation_end2base(pingpong_base[0,0],pingpong_base[1,0],pingpong_base[2,0])
inv_pingpong = invKine(inp_invKine)
# print('IK \n %s \n' %inv_pingpong)
grasping_q = cost_func(inv_pingpong)
# print('Grasping q \n%s\n' %grasping_q)
g_v = grasping_q
print('grasping_q %s' %grasping_q)
fwd_grasping = fwd_kin(g_v, o_unit = 'p')
print('fwd Grasping q %s' %fwd_grasping)
inp = raw_input("Set Home! (Enter to continue)")
set_home(set_duration = 10)
# inp = raw_input("Continue? y/n: ")[0]
# if (inp == 'y'):
# q_ode = odeint(dynamics,x0,t,args=(tau_v,g_v,q0_v,grasping_point,sess))
q_ode = np.load('q_sample1.npy')
# set_home(g_v, 3)
# print('q_ode %s' %q_ode)
t = np.linspace(0.1, 10, 100)
move_dmp_path(q_ode,t)
final_time = time.time()
print("Processing Time : %s" %(final_time-initial_time))
# else:
# print ("Halting program")
# break
# print('\n#############################################\n')
except KeyboardInterrupt:
rospy.signal_shutdown("KeyboardInterrupt")
raise
def main():
with tf.Session() as sess:
sess.run(tf.global_variables_initializer())
def dynamics(x, t, tau_v, g_v, q0_v, grasping_point, sess):
position = grasping_point
s_v = canoSystem(tau_v, t)
feeddict = {
g[0]: g_v[0],
g[1]: g_v[1],
g[2]: g_v[2],
g[3]: g_v[3],
g[4]: g_v[4],
g[5]: g_v[5],
q[0]: x[0],
q[1]: x[1],
q[2]: x[2],
q[3]: x[3],
q[4]: x[4],
q[5]: x[5],
qd[0]: x[6],
qd[1]: x[7],
qd[2]: x[8],
qd[3]: x[9],
qd[4]: x[10],
qd[5]: x[11],
q0[0]: q0_v[0],
q0[1]: q0_v[1],
q0[2]: q0_v[2],
q0[3]: q0_v[3],
q0[4]: q0_v[4],
q0[5]: q0_v[5],
tau: tau_v,
s: s_v,
xg: position[0],
yg: position[1],
zg: position[2]
}
qdd1_v, qdd2_v, qdd3_v, qdd4_v, qdd5_v, qdd6_v = sess.run(
dmpNet, feed_dict=feeddict)
dx = [
x[6], x[7], x[8], x[9], x[10], x[11], qdd1_v, qdd2_v, qdd3_v,
qdd4_v, qdd5_v, qdd6_v
]
return dx
period = 20
t = np.linspace(2, period, 100)
tau_v = float(1) / period
# q0_v = [-0.0003235975848596695, -1.040771786366598, 1.6213598251342773, -0.34193402925600225, 1.5711277723312378, 3.141711950302124]
q0_v = [0, -pi / 2, pi / 2, 0, pi / 2, pi]
v0 = [0, 0, 0, 0, 0, 0]
x0 = []
x0.extend(q0_v)
x0.extend(v0)
print("Initiate Object Detection")
STATE = 'NONE'
_, _, _, last_capture = IntelRealsense.pingpong_detection(
display=False)
T_camera2base = IntelRealsense.transformation_camera2base()
T_end2base = IntelRealsense.transformation_end2base()
gripper = np.array([[0], [0], [0.1], [1]])
Y = np.array([[0], [0], [0], [0], [0], [0]])
X = np.array([[np.random.normal(0, sigma_x)],
[np.random.normal(0, sigma_y)],
[np.random.normal(0, sigma_d)],
[np.random.normal(0, sigma_v_x)],
[np.random.normal(0, sigma_v_y)],
[np.random.normal(0, sigma_v_d)]])
X, P, X_pred = init_state(X, Y, 0)
I = np.identity(6, dtype=float)
H = np.identity(6, dtype=float)
count = 0
global client
try:
rospy.init_node("dmp", anonymous=True, disable_signals=True)
client = actionlib.SimpleActionClient('follow_joint_trajectory',
FollowJointTrajectoryAction)
client.wait_for_server()
print("Connected to server")
parameters = rospy.get_param(None)
index = str(parameters).find('prefix')
if (index > 0):
prefix = str(parameters)[index + len("prefix': '"):(
index + len("prefix': '") +
str(parameters)[index + len("prefix': '"):-1].find("'"))]
for i, name in enumerate(JOINT_NAMES):
JOINT_NAMES[i] = prefix + name
straight_inp = raw_input("Set Straight y/n?")
if straight_inp == 'y':
set_home(straight, 5)
set_home(set_duration=10)
# inp = raw_input("Continue? (Enter to continue)")
while (True):
if STATE == 'GRASPING':
print('X: %s' % X)
dist = X[0, 0]**2 + X[1, 0]**2 + X[2, 0]**2
timestep_stack = 0
while (dist >= 0.81):
timestep_stack += timestep
X = grasping_prediction(X, timestep)
dist = X[0, 0]**2 + X[1, 0]**2 + X[2, 0]**2
print(dist)
print('X: %s (timestep_stack %s)' % (X, timestep_stack))
pingpong_base[0] = X[0]
pingpong_base[1] = X[2]
pingpong_base[2] = X[1]
# print('Real World Coordinates From KF \n %s \n' %pingpong_camera)
# pingpong_base = T_camera2base.dot(pingpong_camera)
print('Ping-Pong Position (Base Frame) \n %s \n' %
pingpong_base)
# grasping_point = [pingpong_base[0], pingpong_base[1], pingpong_base[2]]
# print('grasping point %s' %grasping_point)
grasping_point = [
pingpong_base[0], pingpong_base[1], pingpong_base[2]
]
pingpong_base = pingpong_base - T_end2base.dot(
gripper) # **
inp_invKine = IntelRealsense.transformation_end2base(
pingpong_base[0, 0], pingpong_base[1, 0],
pingpong_base[2, 0])
inv_pingpong = invKine(inp_invKine)
# grasping_q = choose_q(inv_pingpong,0)
# print('IK \n %s \n' %inv_pingpong)
grasping_q = cost_func(inv_pingpong)
# print('Grasping q \n%s\n' %grasping_q)
#### to Kanut DMP ####
g_v = grasping_q
# print('grasping_q %s' %grasping_q)
fwd_grasping = fwd_kin(g_v, o_unit='p')
print('fwd Grasping q %s' % fwd_grasping)
# inp = raw_input("Set Home! (Enter to continue)")
# set_home(set_duration = 10)
# inp = raw_input("Continue? y/n: ")[0]
# if (inp == 'y'):
# q_ode = odeint(dynamics,x0,t,args=(tau_v,g_v,q0_v,grasping_point,sess))
# q_ode = np.load('q.npy')
set_home(g_v, 3)
# print(q_ode)
# move_dmp_path(q_ode,t)
STATE = 'STOP'
time.sleep(2)
set_home(set_duration=10)
# print('q_ode %s' %q_ode)
# move_dmp_path(q_ode,t)
# else:
# print ("Halting program")
# break
elif STATE != 'GRASPING' and STATE != 'STOP':
initial_time = time.time()
pp_x, pp_y, pp_depth, capture = IntelRealsense.pingpong_detection(
display=True) # Positional Detection
pingpong_camera = IntelRealsense.reverse_perspective_projection(
pp_x, pp_y, pp_depth)
pingpong_camera[3] = 1 # Real World Positional Projection
processing = capture - last_capture
timestep = processing # Define timestep values
# print('Distance from camera \n%s' %(pingpong_camera))
pingpong_base = T_camera2base.dot(
pingpong_camera
) # changing reference frame to base frame
# print('Distance from base UR5 %s (%s)' %(pingpong_base, timestep))
v_x, v_depth, v_z, a_x, a_y, a_depth, STATE = IntelRealsense.pingpong_velocity(
STATE,
pingpong_base[0, 0],
pingpong_base[1, 0],
pingpong_base[2, 0],
capture,
display=False) # Velocity Detection
last_capture = capture
#### Kalman Filter Supitcha Klanpradit ####
if STATE != 'NONE':
# print('(x,y,depth,v_x,v_y,v_depth) (%s,%s,%s,%s,%s,%s)' %(pp_x, pp_y, pp_depth, v_x, v_depth, v_z))
Y = np.array([
pingpong_base[0], pingpong_base[2],
pingpong_base[1], [(v_x != 999) * (v_x)],
[(v_z != 999) * (v_z)],
[(v_depth != 999) * (v_depth)]
])
# print('Y : %s, STATE : %s' %(Y, STATE))
if STATE == 'VELOCITY':
X, P, X_pred = init_detection_state(
X, P, Y, timestep)
# print('P: %s' %P)
elif STATE == 'KALMAN':
X, P, X_pred = kalman_filter(X, Y, H, P, timestep)
count += 1
if count >= 3: STATE = 'GRASPING'
# final_time = time.time()
# print("Processing Time : %s" %(final_time-initial_time))
except KeyboardInterrupt:
rospy.signal_shutdown("KeyboardInterrupt")
raise
from trajectory_msgs.msg import *
from sensor_msgs.msg import JointState
from math import pi
import numpy as np
import tensorflow as tf
import numpy as np
from scipy.integrate import odeint
from math import exp
# From Files
from object_detection import IntelRealsense
from universal_robot_kinematics import invKine
from kinematics import fwd_kin
from last_kalman_filter import *
IntelRealsense = IntelRealsense()
JOINT_NAMES = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
home = [0, -pi/2, pi/2, 0, pi/2, pi]
client = None
# Kanut Thummaraksa
#### Input Tensors ####
## Common Input ##
s = tf.placeholder(tf.float64,name='s')
tau = tf.placeholder(tf.float64,name='tau')
xg = tf.placeholder(tf.float64,name='xg')
yg = tf.placeholder(tf.float64,name='yg')
zg = tf.placeholder(tf.float64,name='zg')
def main():
with tf.Session() as sess:
sess.run(tf.global_variables_initializer())
def dynamics(x, t, tau_v, g_v, q0_v, grasping_point, sess):
position = grasping_point
s_v = canoSystem(tau_v, t)
feeddict = {
g[0]: g_v[0],
g[1]: g_v[1],
g[2]: g_v[2],
g[3]: g_v[3],
g[4]: g_v[4],
g[5]: g_v[5],
q[0]: x[0],
q[1]: x[1],
q[2]: x[2],
q[3]: x[3],
q[4]: x[4],
q[5]: x[5],
qd[0]: x[6],
qd[1]: x[7],
qd[2]: x[8],
qd[3]: x[9],
qd[4]: x[10],
qd[5]: x[11],
q0[0]: q0_v[0],
q0[1]: q0_v[1],
q0[2]: q0_v[2],
q0[3]: q0_v[3],
q0[4]: q0_v[4],
q0[5]: q0_v[5],
tau: tau_v,
s: s_v,
xg: position[0],
yg: position[1],
zg: position[2]
}
qdd1_v, qdd2_v, qdd3_v, qdd4_v, qdd5_v, qdd6_v = sess.run(
dmpNet, feed_dict=feeddict)
dx = [
x[6], x[7], x[8], x[9], x[10], x[11], qdd1_v, qdd2_v, qdd3_v,
qdd4_v, qdd5_v, qdd6_v
]
return dx
period = 20
t = np.linspace(5, period, 20)
tau_v = float(1) / period
q0_v = [0, -pi / 2, pi / 2, 0, pi / 2, pi]
v0 = [0, 0, 0, 0, 0, 0]
x0 = []
x0.extend(q0_v)
x0.extend(v0)
print("Initiate Object Detection")
STATE = 'NONE'
_, _, _, last_capture = IntelRealsense.pingpong_detection(
display=False)
T_camera2base = IntelRealsense.transformation_camera2base()
T_end2base = IntelRealsense.transformation_end2base()
gripper = np.array([[0], [0], [0.1], [1]])
Y = np.array([[0], [0], [0], [0], [0], [0]])
X = np.array([[np.random.normal(0, sigma_x)],
[np.random.normal(0, sigma_y)],
[np.random.normal(0, sigma_d)],
[np.random.normal(0, sigma_v_x)],
[np.random.normal(0, sigma_v_y)],
[np.random.normal(0, sigma_v_d)]])
X, P, _ = init_state(X, Y, 0)
print('First State is %s' % X)
I = np.identity(6, dtype=float)
H = np.identity(6, dtype=float)
count = 0
global client
try:
rospy.init_node("dmp", anonymous=True, disable_signals=True)
client = actionlib.SimpleActionClient('follow_joint_trajectory',
FollowJointTrajectoryAction)
client.wait_for_server()
print("Connected to server")
parameters = rospy.get_param(None)
index = str(parameters).find('prefix')
if (index > 0):
prefix = str(parameters)[index + len("prefix': '"):(
index + len("prefix': '") +
str(parameters)[index + len("prefix': '"):-1].find("'"))]
for i, name in enumerate(JOINT_NAMES):
JOINT_NAMES[i] = prefix + name
book = Workbook()
sheet = book.active
excel_row = 1
while (True):
initial_time = time.time()
straight_inp = raw_input("Set Straight y/n?")
if straight_inp == 'y':
set_home(straight, 5)
pp_x, pp_y, pp_depth, capture = IntelRealsense.pingpong_detection(
display=True)
pingpong_camera = IntelRealsense.reverse_perspective_projection(
pp_x, pp_y, pp_depth)
pingpong_camera[3] = 1
processing = capture - last_capture
timestep = processing
pingpong_base = T_camera2base.dot(pingpong_camera)
v_x, v_depth, v_z, a_x, a_y, a_depth, STATE = IntelRealsense.pingpong_velocity(
STATE,
pingpong_base[0, 0],
pingpong_base[1, 0],
pingpong_base[2, 0],
capture,
display=False)
last_capture = capture
# print('Real World Coordinates From KF \n %s \n' %pingpong_camera)
print('Ping-Pong Position (Base Frame) \n %s \n' %
pingpong_base)
grasping_point = [
pingpong_base[0], pingpong_base[1], pingpong_base[2]
]
# print('grasping point %s' %grasping_point)
pingpong_base = pingpong_base - T_end2base.dot(gripper)
inp_invKine = IntelRealsense.transformation_end2base(
pingpong_base[0, 0], pingpong_base[1, 0], pingpong_base[2,
0])
inv_pingpong = invKine(inp_invKine)
print('IK \n %s \n' % inv_pingpong)
# grasping_q = cost_func(inv_pingpong)
grasping_q = choose_q(inv_pingpong, 0)
print('Selected IK \n%s\n' % grasping_q)
g_v = grasping_q
fwd_grasping = fwd_kin(g_v, o_unit='p')
print('Selected FK \n%s\n' % fwd_grasping)
inp = raw_input("Set Home! (Enter to continue)")
set_home(set_duration=10)
# inp = raw_input("Continue? y/n: ")[0]
# if (inp == 'y'):
# q_ode = odeint(dynamics,x0,t,args=(tau_v,g_v,q0_v,grasping_point,sess))
# q_ode = np.load('q_sample2.npy')
set_home(g_v, 3)
# print('q_ode %s' %q_ode)
# move_dmp_path(q_ode,t)
final_time = time.time()
print("Processing Time : %s" % (final_time - initial_time))
# else:
# print ("Halting program")
# break
# print('\n#############################################\n')
except KeyboardInterrupt:
rospy.signal_shutdown("KeyboardInterrupt")
raise