def main():
global rgb_img
global depth_img
rospy.init_node('d_points', anonymous=True)
rospy.Subscriber('/camera/rgb/image_raw', Image, rgb_callback)
rospy.Subscriber('/camera/depth/image_raw', Image, d_callback)
pub = rospy.Publisher('3_point_features', floatList, queue_size=50)
rate = rospy.Rate(100) # in Hz
features = floatList()
while not rospy.is_shutdown():
xy = xy_points(rgb_img)
c = 0
while c < 20:
xy = xy + xy_points(rgb_img)
c += 1
xy = xy / c
features.data = f_points(depth_img, xy)
print(features.data)
pub.publish(features)
rate.sleep()
def main():
global rgb_img
global depth_img
global trackers
x = 0
rospy.init_node('d_points', anonymous=True)
rospy.Subscriber('/camera/rgb/image_raw', Image, rgb_callback)
rospy.Subscriber('/camera/depth/image_raw', Image, d_callback)
pub = rospy.Publisher('four_point_features', floatList, queue_size=1)
features = floatList()
while not rospy.is_shutdown():
while x != 1:
x = camshift(rgb_img, depth_img)
if len(x) < 1:
print "empty"
else:
x = np.asarray(x)
c = 0
while c < 9:
x = add_lists(x, camshift(rgb_img, depth_img))
c += 1
c = c + 1
x = div_lists(x, c)
features.data = x
print features.data
pub.publish(features)
else:
cv2.destroyAllWindows()
break
def vs_ur5():
global joint_positions
global coordinates
global j_velocity
data_file = open("visual_servo_data.txt", "w+")
data_file.write("New Data: \n")
data_file.write("\n")
rospy.init_node('vs_ur5')
rospy.Subscriber('/four_point_features',
floatList,
coordinate_callback,
queue_size=1)
rospy.Subscriber("/joint_states",
JointState,
position_callback,
queue_size=1)
#Creating the publisher
cam_pub = rospy.Publisher('ur5_joint_velocities', floatList, queue_size=10)
error_pub = rospy.Publisher('points_error', floatList, queue_size=10)
while not rospy.is_shutdown():
total_data = joint_velocities(joint_positions, coordinates)
if total_data != None:
data_file.write("Error: \n" + str(total_data[0]) + "\n")
data_file.write("Points: \n" + str(total_data[1]) + "\n")
data_file.write("Camera Velocity: \n" + str(total_data[1]) + "\n")
data_file.write("Joint Velocity: \n" + str(total_data[1]) + "\n")
data_file.write("\n")
data_file.write("\n")
# print total_data
# final_error_list=[]
error_list = total_data[0]
j_velocity = total_data[3]
# for index in range (0,len(error_list),2):
# x = (error_list[index]**2+error_list[index+1])**0.5 #computes the average error for a point
# final_error_list.append(x)
new_List = floatList()
new_List.data = j_velocity
new_List.header.stamp = rospy.Time.now()
error_l = floatList()
error_l.header.stamp = rospy.Time.now(
) #gives the timestamp of the error message as header
error_l.data = error_list
cam_pub.publish(new_List) # publishes joint velocity
error_pub.publish(error_l) # publishes point error
data_file.close()
def talker():
pub = rospy.Publisher('ur5_joint_velocities', floatList, queue_size=10)
rospy.init_node('talker', anonymous=True)
vels = floatList()
vels.data = [0.05, 0, 0, 0, 0, 0]
while not rospy.is_shutdown():
# print rospy.Time.now()
pub.publish(vels)
def tumbling_vs_ur5():
global joint_positions
global ellipse_data
global depth_img
rospy.init_node('tumbling_vs_ur5')
rospy.Subscriber("/ellipse_param", String, ellipse_params_callback,queue_size=1)
rospy.Subscriber('/camera/depth/image_raw', Image, depth_callback)
rospy.Subscriber("/joint_states", JointState, position_callback,queue_size=1)
cam_pub=rospy.Publisher('ur5_joint_velocities',floatList,queue_size=10)
while not rospy.is_shutdown():
new_List = floatList()
new_List.data = joint_velocities(ellipse_data,depth_img,joint_positions)
print new_List.data
cam_pub.publish(new_List)
def vs_ur5():
global joint_positions
global coordinates
global j_velocity
rospy.init_node('vs_ur5')
rospy.Subscriber('/3_point_features', floatList,coordinate_callback,queue_size=1)
rospy.Subscriber("/joint_states", JointState, position_callback,queue_size=1)
#Creating the publisher
cam_pub=rospy.Publisher('ur5_joint_velocities',floatList,queue_size=10)
while not rospy.is_shutdown():
j_velocity = joint_velocities(joint_positions,coordinates)
new_List = floatList()
new_List.data = j_velocity
cam_pub.publish(new_List)
def main():
global rgb_img
global depth_img
rospy.init_node('d_points', anonymous=True)
rospy.Subscriber('/camera/rgb/image_raw', Image, rgb_callback)
rospy.Subscriber('/camera/depth/image_raw', Image, d_callback)
pub = rospy.Publisher('four_point_features', floatList, queue_size=1)
features = floatList()
while not rospy.is_shutdown():
xy = xy_points(rgb_img)
c = 0
while c < 20: #calculates average feature coordinates for every 20 iterations to reduce fluctuations
xy = xy + xy_points(rgb_img)
c += 1
xy = xy / c
features.data = f_points(depth_img, xy)
features.header.stamp = rospy.Time.now()
print "Green, Blue, Red, Orange :"
print(features.data)
pub.publish(features)
def joint_velocities(pose,points):
if pose!=None:
if points!=None:
theta_x = -pi/2
theta_y = pi/2
int_matrix = None
fl = 531.15
desired_points = [93.99359819243081, -51.51572208623612, 837, -84.35322914705328, 56.03464507625683, 814, 100.01882884579176, 55.130860478252686, 817]
desired_points = np.asarray(desired_points)
points=np.asarray(points)
pix_vel=[]
for index in range(len(points)):
if (index+1)%3==0:
index+=1
else:
pix_vel.append(points[index]-desired_points[index])
# print pix_vel
pix_vel=np.asarray(pix_vel) #1v8 array
pix_vel=np.matrix(pix_vel)
pix_vel=np.ndarray.transpose(pix_vel)
for i in range(0,len(points),3):
u = desired_points[i]
v = desired_points[i+1]
z = desired_points[i+2]
new_mat=np.array([[ -fl/z, 0, u/z, u*v/fl, -(fl*fl+u*u)/fl, v],[ 0, -fl/z, v/z, (fl*fl+v*v)/fl, -u*v/fl, -u ]])
# new_mat=np.array([[ 0, 0, u/z,0,0,0],[ 0, 0, v/z, 0,0,0 ]])
if i==0:
int_matrix=new_mat
else:
int_matrix=np.concatenate((int_matrix,new_mat))
int_matrix=np.matrix(int_matrix) #8v6 matrix
inverse=np.linalg.pinv(int_matrix) #6v8 matrix
cam_vel_np=-0.000005*inverse*pix_vel #6v1 matrix
cam_vel_lin = cam_vel_np[[0,1,2],:]
cam_vel_angular = cam_vel_np[[3,4,5],:]
Rx=np.array([[1,0,0],[0,cos(theta_x),-sin(theta_x)],[0,sin(theta_x),cos(theta_x)]])
Ry=np.array([[cos(theta_y) , 0, sin(theta_y)],[0, 1, 0],[-sin(theta_y), 0, cos(theta_y)]])
# Lt=np.array([[1,0,0,-210],[0,1,0,0],[0,0,1,-85],[0,0,0,1]])
# # print Rx
# # print Ry
# cam_vel_lin=Ry*cam_vel_lin
# cam_vel_lin=Rx*cam_vel_lin
# cam_vel_lin=np.vstack((cam_vel_lin,[1]))
# cam_vel_lin=Lt*cam_vel_lin
# cam_vel_lin=np.delete(cam_vel_lin,(3),axis=0)
# cam_vel_angular=Rx*(Ry*cam_vel_angular)
# cam_vel_angular=np.vstack((cam_vel_angular,[1]))
# cam_vel_angular=Lt*cam_vel_angular
# cam_vel_angular=np.delete(cam_vel_angular,(3),axis=0)
cam_vel_np=np.concatenate((cam_vel_lin,cam_vel_angular))
# cam_vel_np[2]=-cam_vel_np[2]
# cam_vel_np[5]=-cam_vel_np[5]
# cam_vel_np=np.array([[0],[0],[0],[0],[0],[0]])
# cam_vel_np=np.ndarray.transpose(cam_vel_np)
# cam_vel_list=np.ndarray.tolist(cam_vel_np)
# cam_vel_final=cam_vel_list[0]
temp1 = cam_vel_np.item((0,0))
temp2 = cam_vel_np.item((1,0))
temp3 = cam_vel_np.item((2,0))
temp4 = cam_vel_np.item((3,0))
temp5 = cam_vel_np.item((4,0))
temp6 = cam_vel_np.item((5,0))
# cam_vel_np[0,0] = temp3
# cam_vel_np[1,0] = -temp1
# cam_vel_np[2,0] = temp2
# cam_vel_np[3,0] = temp6
# cam_vel_np[4,0] = -temp4
# cam_vel_np[5,0] = temp5
cam_vel_np[0,0] = temp3
cam_vel_np[1,0] = temp1
cam_vel_np[2,0] = temp2
cam_vel_np[3,0] = temp6
cam_vel_np[4,0] = temp4
cam_vel_np[5,0] = temp5
print cam_vel_np
Q1=pose[0]
Q2=pose[1]
Q3=pose[2]
Q4=pose[3]
Q5=pose[4]
Q6=pose[5]
jacobian=np.array([[ (2183*np.cos(Q1))/20000 + (823*np.cos(Q1)*np.cos(Q5))/10000 + (17*np.cos(Q2)*np.sin(Q1))/40 - (1569*np.sin(Q1)*np.sin(Q2)*np.sin(Q3))/4000 + (823*np.cos(Q2 + Q3 + Q4)*np.sin(Q1)*np.sin(Q5))/10000 - (591*np.cos(Q2 + Q3)*np.sin(Q1)*np.sin(Q4))/6250 - (591*np.sin(Q2 + Q3)*np.cos(Q4)*np.sin(Q1))/6250 + (1569*np.cos(Q2)*np.cos(Q3)*np.sin(Q1))/4000, np.cos(Q1)*((1569*np.sin(Q2 + Q3))/4000 + (17*np.sin(Q2))/40 + np.sin(Q5)*((823*np.cos(Q2 + Q3)*np.sin(Q4))/10000 + (823*np.sin(Q2 + Q3)*np.cos(Q4))/10000) + (591*np.cos(Q2 + Q3)*np.cos(Q4))/6250 - (591*np.sin(Q2 + Q3)*np.sin(Q4))/6250), np.cos(Q1)*((591*np.cos(Q2 + Q3 + Q4))/6250 + (1569*np.sin(Q2 + Q3))/4000 + (823*np.sin(Q2 + Q3 + Q4)*np.sin(Q5))/10000), np.cos(Q1)*((591*np.cos(Q2 + Q3 + Q4))/6250 + (823*np.sin(Q2 + Q3 + Q4)*np.sin(Q5))/10000), (823*np.cos(Q1)*np.cos(Q2)*np.cos(Q5)*np.sin(Q3)*np.sin(Q4))/10000 - (823*np.cos(Q1)*np.cos(Q2)*np.cos(Q3)*np.cos(Q4)*np.cos(Q5))/10000 - (823*np.sin(Q1)*np.sin(Q5))/10000 + (823*np.cos(Q1)*np.cos(Q3)*np.cos(Q5)*np.sin(Q2)*np.sin(Q4))/10000 + (823*np.cos(Q1)*np.cos(Q4)*np.cos(Q5)*np.sin(Q2)*np.sin(Q3))/10000, 0],[ (2183*np.sin(Q1))/20000 - (17*np.cos(Q1)*np.cos(Q2))/40 + (823*np.cos(Q5)*np.sin(Q1))/10000 - (823*np.cos(Q2 + Q3 + Q4)*np.cos(Q1)*np.sin(Q5))/10000 + (591*np.cos(Q2 + Q3)*np.cos(Q1)*np.sin(Q4))/6250 + (591*np.sin(Q2 + Q3)*np.cos(Q1)*np.cos(Q4))/6250 - (1569*np.cos(Q1)*np.cos(Q2)*np.cos(Q3))/4000 + (1569*np.cos(Q1)*np.sin(Q2)*np.sin(Q3))/4000, np.sin(Q1)*((1569*np.sin(Q2 + Q3))/4000 + (17*np.sin(Q2))/40 + np.sin(Q5)*((823*np.cos(Q2 + Q3)*np.sin(Q4))/10000 + (823*np.sin(Q2 + Q3)*np.cos(Q4))/10000) + (591*np.cos(Q2 + Q3)*np.cos(Q4))/6250 - (591*np.sin(Q2 + Q3)*np.sin(Q4))/6250), np.sin(Q1)*((591*np.cos(Q2 + Q3 + Q4))/6250 + (1569*np.sin(Q2 + Q3))/4000 + (823*np.sin(Q2 + Q3 + Q4)*np.sin(Q5))/10000), np.sin(Q1)*((591*np.cos(Q2 + Q3 + Q4))/6250 + (823*np.sin(Q2 + Q3 + Q4)*np.sin(Q5))/10000), (823*np.cos(Q1)*np.sin(Q5))/10000 - (823*np.cos(Q2)*np.cos(Q3)*np.cos(Q4)*np.cos(Q5)*np.sin(Q1))/10000 + (823*np.cos(Q2)*np.cos(Q5)*np.sin(Q1)*np.sin(Q3)*np.sin(Q4))/10000 + (823*np.cos(Q3)*np.cos(Q5)*np.sin(Q1)*np.sin(Q2)*np.sin(Q4))/10000 + (823*np.cos(Q4)*np.cos(Q5)*np.sin(Q1)*np.sin(Q2)*np.sin(Q3))/10000, 0],[ 0, (591*np.sin(Q2 + Q3 + Q4))/6250 - (823*np.sin(Q2 + Q3 + Q4 + Q5))/20000 - (1569*np.cos(Q2 + Q3))/4000 - (17*np.cos(Q2))/40 + (823*np.sin(Q2 + Q3 + Q4 - Q5))/20000, (591*np.sin(Q2 + Q3 + Q4))/6250 - (823*np.sin(Q2 + Q3 + Q4 + Q5))/20000 - (1569*np.cos(Q2 + Q3))/4000 + (823*np.sin(Q2 + Q3 + Q4 - Q5))/20000, (591*np.sin(Q2 + Q3 + Q4))/6250 - (823*np.sin(Q2 + Q3 + Q4 + Q5))/20000 + (823*np.sin(Q2 + Q3 + Q4 - Q5))/20000, - (823*np.sin(Q2 + Q3 + Q4 + Q5))/20000 - (823*np.sin(Q2 + Q3 + Q4 - Q5))/20000, 0],[ 0, np.sin(Q1), np.sin(Q1), np.sin(Q1), np.sin(Q2 + Q3 + Q4)*np.cos(Q1), np.cos(Q5)*np.sin(Q1) - np.cos(Q2 + Q3 + Q4)*np.cos(Q1)*np.sin(Q5)],[ 0, -np.cos(Q1), -np.cos(Q1), -np.cos(Q1), np.sin(Q2 + Q3 + Q4)*np.sin(Q1), - np.cos(Q1)*np.cos(Q5) - np.cos(Q2 + Q3 + Q4)*np.sin(Q1)*np.sin(Q5)],[ 1, 0, 0, 0, -np.cos(Q2 + Q3 + Q4), -np.sin(Q2 + Q3 + Q4)*np.sin(Q5)]])
jacobian=np.matrix(jacobian)
jacobian=np.linalg.pinv(jacobian)
joint_vel=jacobian*cam_vel_np
joint_vel=joint_vel
joint_vel=np.ndarray.transpose(joint_vel)
joint_vel=joint_vel.tolist()
joint_vel=joint_vel[0]
joint_vel=tuple(joint_vel)
print "Joint Velocity: "
print joint_vel
#Creating the publisher
cam_pub=rospy.Publisher('ur5_joint_velocities',floatList,queue_size=10)
new_List=floatList()
new_List.data=joint_vel
# print new_List
rate = rospy.Rate(100) #20 Hz
cam_pub.publish(new_List)
rate.sleep()
