def main():
# start socket
TCP_IP = '128.237.198.49'
TCP_PORT = 2002
print('Socket Information: %s:%d' % (TCP_IP, TCP_PORT))
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.connect((TCP_IP, TCP_PORT))
time.sleep(1e-3)
# start camera
vs = VideoStream(src=0).start()
time.sleep(2.0)
# calibration and find block
#Caliberate_camera(vs)
block_pixel_position = detect_block_grab(vs)
block_real_position = transfer_to_real(block_pixel_position)
print(block_pixel_position)
print(block_real_position)
# Inverse Kinematics
inverse_kinematics(block_real_position,s)
HOME_POSITION = [20,-15,20]
roll = -math.pi/2
traj = RPC.pipline_position_encoder_roll(HOME_POSITION, [40,-30,20], roll, s)
traj = RPC.pipline_position_encoder_roll([40,-30,20], [40,-30,12], roll, s)
Pipeline.C_execute([traj])
the_block = [42, -26, 1, -1.04]
ball_position = detect_ball(vs)
ball_position.reverse()
K = 0.4
adj = 0
for pos in ball_position:
if pos == None:
continue
print(pos)
if pos[1] > -16:
if pos[0] > 47:
err = pos[0]-47
adj = err*K
break
print(adj)
#print(ball_position)
commands = Pipeline.Adjust(the_block, adj, s)
for command in commands:
print(command)
Pipeline.C_execute(commands)
'
def inverse_kinematics(block_real_position, s):
put_index = 0
for key in block_real_position:
grab_position = block_real_position[key]
if(grab_position[2] > 0):
grab_roll = grab_position[2] - 180
else:
grab_roll = grab_position[2]
grab_roll = grab_roll/180*math.pi
grab_position = [grab_position[0][0]-5,grab_position[0][1],1]
put_position = block_put_position[put_index]
put_roll = put_position[3]
put_position = put_position[0:3]
commands = Pipeline.classical_combi(grab_position, put_position, grab_roll, put_roll, s)
Pipeline.C_execute(commands)
put_index += 1
HOME_POSITION = [20,-15,20]
END_POSITION = [40,20,20]
traj = RPC.pipline_position_encoder(HOME_POSITION,END_POSITION,s)
Pipeline.C_execute([traj])
