Exemplo n.º 1
0
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
Exemplo n.º 2
0
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
Exemplo n.º 3
0
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
Exemplo n.º 4
0
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
Exemplo n.º 5
0
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
Exemplo n.º 6
0
        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)


if __name__ == '__main__':
    # defining tx address/mode
    address_tx = (ms_com_ip, kin_link_0)
    mode_tx = "udp"

    # initialize
    obj_tx = transfers.init_tx(address_tx, mode_tx)

    # collect events until released
    with mouse.Listener(on_move=on_move,
                        on_click=on_click,
                        on_scroll=on_scroll) as listener:
        listener.join()

    # exit
    transfers.close(obj_tx)
Exemplo n.º 7
0
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