def backward_flow_haptic():
### defining in/out address/mode
address_rx = ms_com_bwd_flow_haptic_entry_addr
address_tx = ms_com_bwd_flow_haptic_exit_addr
mode_rx = ms_com_bwd_flow_haptic_entry_mode
mode_tx = ms_com_bwd_flow_haptic_exit_mode
### initialization
obj_tx = transfers.init_tx(address_tx, mode_tx)
obj_rx = transfers.init_rx(address_rx, mode_rx)
while (1):
print "live_mscom_bwd_haptic", time.time()
### receive message
msg = transfers.receive(obj_rx)
### decode the message
msg_list = codec.generic.decode(msg)
print "force feedback", msg_list
### code message
msg = codec.generic.codev2(msg, msg_list)
### send message
transfers.send(obj_tx, msg)
transfers.close(obj_tx)
transfers.close(obj_rx)
return
def backward_flow_haptic():
### defining in/out address/mode
address_rx = server_bwd_flow_haptic_entry_addr
address_tx = server_bwd_flow_haptic_exit_addr
mode_rx = server_bwd_flow_haptic_entry_mode
mode_tx = server_bwd_flow_haptic_exit_mode
### initialization
obj_tx = transfers.init_tx(address_tx, mode_tx)
obj_rx = transfers.init_rx(address_rx, mode_rx)
while (1):
print "live_srv_bwd", time.time()
### receive message
msg = transfers.receive(obj_rx)
### decode the messasge
msg_list = codec.generic.decode(
msg) # msg_list = [force-sensor-1-data, force-sensor-2-data]
### encode the message
msg = codec.generic.codev2(msg, msg_list)
### send the message
transfers.send(obj_tx, msg)
transfers.close(obj_tx)
transfers.close(obj_rx)
return
def forward_flow_kinematic():
### defining in/out address/mode
address_rx = ss_com_fwd_flow_kinematic_entry_addr
address_tx = ss_com_fwd_flow_kinematic_exit_addr
mode_rx = ss_com_fwd_flow_kinematic_entry_mode
mode_tx = ss_com_fwd_flow_kinematic_exit_mode
### initialization
obj_tx = transfers.init_tx(address_tx,mode_tx)
obj_rx = transfers.init_rx(address_rx,mode_rx)
while (1):
print "live_sscom_fwd",time.time()
### receive message
msg = transfers.receive(obj_rx)
ping.echo_back_rev2(msg,"tpf_ss_com_entry",address_rx)
### decode message
msg_list = codec.generic.decode(msg)
### encode message
msg = codec.generic.codev2(msg,msg_list)
### send to tactile slave
transfers.send(obj_tx,msg)
ping.echo_back_rev2(msg,"tpf_ss_com_exit",address_rx)
transfers.close(obj_tx)
transfers.close(obj_rx)
return
def backward_flow_haptic():
### defining in/out address/mode
address_rx = ss_com_bwd_flow_haptic_entry_addr
address_tx = ss_com_bwd_flow_haptic_exit_addr
mode_rx = ss_com_bwd_flow_haptic_entry_mode
mode_tx = ss_com_bwd_flow_haptic_exit_mode
### initialization
obj_tx = transfers.init_tx(address_tx, mode_tx)
obj_rx = transfers.init_rx(address_rx, mode_rx)
while (1):
print "live_sscom_bwd", time.time()
### receive message
msg = transfers.receive(obj_rx)
ping.echo_back(msg, "tpf_ss_embsys_entry") #ss_embsys tp's
ping.echo_back(msg, "tpf_ss_embsys_exit")
ping.echo_back(msg, "tpb_ss_embsys_entry")
ping.echo_back(msg, "tpb_ss_embsys_exit")
ping.echo_back(msg, "tpb_sscom_bwd_entry")
### send message
transfers.send(obj_tx, msg)
ping.echo_back(msg, "tp_sscom_bwd_exit")
transfers.close(obj_tx)
transfers.close(obj_rx)
return
def on_click(x, y, button, pressed):
global axis_x
global axis_y
global axis_z
global button_left
global button_right
global scroll
if ((button == button.left) and (pressed == True)):
button_left = 1
if ((button == button.left) and (pressed == False)):
button_left = 0
if ((button == button.right) and (pressed == True)):
button_right = 1
if ((button == button.right) and (pressed == False)):
button_right = 0
print axis_x, axis_y, axis_z, button_left, button_right, scroll
msg = message_format(axis_x, axis_y, axis_z, button_left, button_right,
scroll)
transfers.send(obj_tx, msg)
def forward_flow_kinematic():
### defining in/out address/mode
address_rx = server_fwd_flow_kinematic_entry_addr
address_tx = server_fwd_flow_kinematic_exit_addr
mode_rx = server_fwd_flow_kinematic_entry_mode
mode_tx = server_fwd_flow_kinematic_exit_mode
### initialization
obj_tx = transfers.init_tx(address_tx, mode_tx)
obj_rx = transfers.init_rx(address_rx, mode_rx)
while (1):
print "live_srv_fwd", time.time()
### receive message
msg = transfers.receive(obj_rx)
ping.echo_back_rev2(msg, "tpf_srv_entry", address_rx)
### decode message
msg_list = codec.generic.decode(msg) #msg_list:(x,y,z, pitch, pincher)
ping.echo_back_rev2(msg, "tpf_srv_1", address_rx)
### run kinematics algorithm (here)
msg_list = algorithms.kinematics.mouse_controller.revGeneric(
msg_list) # msg-list: (x,y,z,pitch=0,pincher)
ping.echo_back_rev2(msg, "tpf_srv_2", address_rx)
### run inverse kinematics algorithm
msg_list = algorithms.inverse_kinematics.vrepPhantomX.py_fsolve(
msg_list) #msg_list:(joint1, joint2, joint3, joint4, gripperPos)
ping.echo_back_rev2(msg, "tpf_srv_3", address_rx)
### code the message
msg = codec.generic.codev2(msg, msg_list)
ping.echo_back_rev2(msg, "tpf_srv_4", address_rx)
### send message
transfers.send(obj_tx, msg)
ping.echo_back_rev2(msg, "tpf_srv_exit", address_rx)
transfers.close(obj_tx)
transfers.close(obj_rx)
return
def on_move(x, y):
global axis_x
global axis_y
global axis_z
global button_left
global button_right
global scroll
global obj_tx
axis_x = x
axis_y = y
print axis_x, axis_y, axis_z, button_left, button_right, scroll
msg = message_format(axis_x, axis_y, axis_z, button_left, button_right,
scroll)
transfers.send(obj_tx, msg)
def on_scroll(x, y, dx, dy):
global axis_x
global axis_y
global axis_z
global button_left
global button_right
global scroll
if (dy < 0):
scroll = scroll - 1
if (scroll < 0):
scroll = 0
else:
scroll = scroll + 1
if (scroll > 25):
scroll = 25
print axis_x, axis_y, axis_z, button_left, button_right, scroll
msg = message_format(axis_x, axis_y, axis_z, button_left, button_right,
scroll)
transfers.send(obj_tx, msg)
def vrepControl():
dataForce = 0
### defining in/out address/mode
address_rx = (ss_embsys_app_ip, kin_link_3)
address_tx = (ss_com_ip, hap_link_0)
mode_rx = "udp"
mode_tx = "udp"
### initialization
obj_rx = transfers.init_rx(address_rx, mode_rx)
obj_tx = transfers.init_tx(address_tx, mode_tx)
while (1):
print "vrepControl", time.time()
### receive message
msg = transfers.receive(obj_rx)
ping.echo_back(msg, "tpf_ss_embsys_entry") #ss_embsys tp's
### decode message
msg_list = codec.generic.decode(msg)
print msg_list
### moving actuators
msg_list = map(int, msg_list)
print "received", msg_list
jointAngle1 = msg_list[0]
jointAngle2 = msg_list[1]
jointAngle3 = msg_list[2]
jointAngle4 = msg_list[3]
gripperPosition = msg_list[4] #0-100, Full Close=0, Full Open = 100
errorcode = vrep.simxSetJointTargetPosition(clientID, jointHandle_1,
jointAngle1 * 3.14 / 180,
vrep.simx_opmode_streaming)
errorcode = vrep.simxSetJointTargetPosition(clientID, jointHandle_2,
jointAngle2 * 3.14 / 180,
vrep.simx_opmode_streaming)
errorcode = vrep.simxSetJointTargetPosition(clientID, jointHandle_3,
jointAngle3 * 3.14 / 180,
vrep.simx_opmode_streaming)
errorcode = vrep.simxSetJointTargetPosition(clientID, jointHandle_4,
jointAngle4 * 3.14 / 180,
vrep.simx_opmode_streaming)
errorcode = vrep.simxSetIntegerSignal(clientID,
'PhantomXPincher_gripperClose',
gripperPosition,
vrep.simx_opmode_oneshot)
### reading haptic data
[errorcode,
dataForce] = vrep.simxGetJointForce(clientID, jointHandle_4,
vrep.simx_opmode_streaming)
dataForce = -1 * dataForce
### Restricting pressure between 0 and 100
if (dataForce < 0):
dataForce = 0
if (dataForce > 100):
dataForce = 100
dataForce = int(100 * dataForce)
print "force", dataForce
### coding haptic data
msg_list = [dataForce]
msg = codec.generic.codev2(msg, msg_list)
### send the message
transfers.send(obj_tx, msg)
ping.echo_back(msg, "tpf_ss_embsys_exit")
transfers.close(obj_tx)
transfers.close(obj_rx)
return