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)
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)
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
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
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
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)
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)
