예제 #1
0
def gen_msg(broker, fnumber, new_pos, ctrl_mode):
    msg = pab.target_pos_msg()
    msg.set_timestamp(time.time())
    msg.set_ctrl_t(ctrl_mode)
    msg.set_fnumber(fnumber)
    msg.set_pos(new_pos)
    msg.set_time_to_go(0.2)
    broker.send_msg("franka_target_pos", msg)
예제 #2
0
def f_msg(pos):
    msg = pab.target_pos_msg()
    msg.set_ctrl_t(pab.CtrlType.Cartesian)
    msg.set_pos(pos)
    msg.set_timestamp(time.clock_gettime(time.CLOCK_MONOTONIC))
    msg.set_fnumber(counter)
    msg.set_time_to_go(time2go)
    b.send_msg("franka_target_pos", msg)
예제 #3
0
def pos_j_msg(pos, broker, counter, time2go=4):
    msg = pab.target_pos_msg()
    msg.set_ctrl_t(pab.CtrlType.Joint)
    msg.set_pos(pos)
    msg.set_timestamp(time.clock_gettime(time.CLOCK_MONOTONIC))
    msg.set_fnumber(counter)
    msg.set_time_to_go(time2go)
    broker.send_msg("franka_target_pos", msg)
    counter += 1
    return counter
예제 #4
0
def create_message(counter=1000, timestamp=time.CLOCK_MONOTONIC, \
                   ctrl_t=0, pos = np.array([0.5, 0.7, 0.7]), go_time=0.15):
    msg = pab.target_pos_msg()
    msg.set_fnumber(counter)
    msg.set_timestamp(timestamp)
    msg.set_ctrl_t(ctrl_t)
    msg.set_pos(pos)
    msg.set_time_to_go(go_time)
    current_counter = counter
    return msg, current_counter
예제 #5
0
def default_pos():
    pos_c = np.array([6.23963393e-01, -5.11126213e-06, 5.59791033e-01])
    msg = pab.target_pos_msg()
    msg.set_ctrl_t(pab.CtrlType.Cartesian)
    msg.set_pos(pos_c)
    msg.set_timestamp(time.clock_gettime(time.CLOCK_MONOTONIC))
    msg.set_fnumber(counter)
    msg.set_time_to_go(1.0)
    b.send_msg("franka_target_pos", msg)
    time.sleep(0.5)
    return None
예제 #6
0
def set_new_pos(broker, new_pos, ctrl_mode, time_to_go):
    # substitute for static counter (for the fnumber)
    if not hasattr(set_new_pos, "fnumber"):
        broker.register_signal("franka_target_pos", pab.MsgType.target_pos)
        set_new_pos.fnumber = 0  # it doesn't exist yet, so initialize it
    msg = pab.target_pos_msg()
    msg.set_timestamp(time.monotonic())
    msg.set_ctrl_t(ctrl_mode)  # 0:cartesian space, 1:joint space
    msg.set_fnumber(set_new_pos.fnumber)
    #msg.set_fnumber(fnumber)
    set_new_pos.fnumber += 1
    if type(new_pos) is not np.array:
        new_pos = np.asarray(new_pos)
    msg.set_pos(new_pos)
    msg.set_time_to_go(time_to_go)
    broker.send_msg("franka_target_pos", msg)
예제 #7
0
while True:
    default_pos()
    counter += 1
    print(counter)

    for z in np.arange(current_pos_c[2], current_pos_c[2] + R, delta_z):
        for theta in np.arange(0, 2 * np.pi, delta_theta):

            phi = np.arccos((z - current_pos_c[2]) / R)
            print(theta, z, phi)

            pos_c = np.array([
                R * np.cos(theta) * np.cos(phi),
                R * np.sin(theta) * np.cos(phi), R * np.sin(phi)
            ])

            msg = pab.target_pos_msg()
            msg.set_ctrl_t(pab.CtrlType.Cartesian)
            msg.set_pos(pos_c)
            msg.set_timestamp(time.clock_gettime(time.CLOCK_MONOTONIC))
            msg.set_fnumber(counter)
            msg.set_time_to_go(time2go)

            b.send_msg("franka_target_pos", msg)
            time.sleep(0.5)
            new_state = b.recv_msg("franka_state", -1)
            print("next pos: {}".format(new_state.get_c_pos()))
            counter += 1
            print(counter)