target_xyz[2] = np.random.uniform(0.3, 0.4)
# update the position of the target
interface.set_mocap_xyz("target", target_xyz)
generated_path = path_planner.generate_path(
start_position=hand_xyz,
target_position=target_xyz,
max_velocity=1,
plot=False,
)
pos_path = generated_path[:, :3]
vel_path = generated_path[:, 3:6]
if use_wall_clock:
pos_path = path_planner.convert_to_time(
path=pos_path, time_length=run_time
)
vel_path = path_planner.convert_to_time(
path=vel_path, time_length=run_time
)
# get next target along trajectory
if use_wall_clock:
target = [function(min(time_elapsed, run_time)) for function in pos_path]
target_velocity = [
function(min(time_elapsed, run_time)) for function in vel_path
]
else:
next_target = path_planner.next()
target = next_target[:3]
target_velocity = next_target[3:]
time_elapsed = 0.0
target_xyz = np.array(
[np.random.random() * 2 - 1,
np.random.random() * 2 + 1, 0])
# update the position of the target
interface.set_target(target_xyz)
generated_path = path_planner.generate_path(
start_position=hand_xyz,
target_position=target_xyz,
max_velocity=1,
plot=False,
)
if use_wall_clock:
pos_path = path_planner.convert_to_time(
path=generated_path[:, :3],
time_length=path_planner.time_to_converge,
)
vel_path = path_planner.convert_to_time(
path=generated_path[:, 3:6],
time_length=path_planner.time_to_converge,
)
# get next target along trajectory
if use_wall_clock:
target = [
function(min(path_planner.time_to_converge, time_elapsed))
for function in pos_path
]
target_velocity = [
function(min(path_planner.time_to_converge, time_elapsed))
for function in vel_path
np.random.random() * 2 + 1, 0])
# update the position of the target
interface.set_target(target_xyz)
generated_path = path_planner.generate_path(
start_position=hand_xyz,
target_position=target_xyz,
max_velocity=3,
plot=False,
)
pos_path = generated_path[:, :3]
vel_path = generated_path[:, 3:6]
if use_wall_clock:
pos_path = path_planner.convert_to_time(
pregenerated_path=pos_path,
time_limit=path_planner.time_to_converge)
vel_path = path_planner.convert_to_time(
pregenerated_path=vel_path,
time_limit=path_planner.time_to_converge)
# get next target along trajectory
if use_wall_clock:
target = [function(time_elapsed) for function in pos_path]
target_velocity = [function(time_elapsed) for function in vel_path]
else:
next_target = path_planner.next()
target = next_target[:3]
target_velocity = next_target[3:]
# generate an operational space control signal