pregenerated_path=pos_path, time_limit=run_time)
vel_path = path_planner.convert_to_time(
pregenerated_path=vel_path, time_limit=run_time)
# get next target along trajectory
if use_wall_clock:
target = [function(time_elapsed) for function in pos_path]
target_vel = [function(time_elapsed) for function in vel_path]
else:
target, target_vel = path_planner.next()
# generate an operational space control signal
u = ctrlr.generate(
q=feedback['q'],
dq=feedback['dq'],
target=np.hstack((target, np.zeros(3))),
target_vel=np.hstack((target_vel, np.zeros(3))),
)
# apply the control signal, step the sim forward
interface.send_forces(u, update_display=(count % 20 == 0))
count += 1
time_elapsed += timeit.default_timer() - start
finally:
# stop and reset the simulation
interface.disconnect()
print('Simulation terminated...')
# get arm feedback
feedback = interface.get_feedback()
hand_xyz = robot_config.Tx('EE', feedback['q'])
# generate an operational space control signal
u = ctrlr.generate(q=feedback['q'],
dq=feedback['dq'],
target_pos=target_xyz)
# add in obstacle avoidance
obs_xy = interface.get_mousexy()
if obs_xy is not None:
avoid.set_obstacles(obstacles=[[obs_xy[0], obs_xy[1], 0, .2]])
u += avoid.generate(q=feedback['q'])
# apply the control signal, step the sim forward
interface.send_forces(u)
# change target location once hand is within
# 5mm of the target
if (np.sqrt(np.sum((target_xyz - hand_xyz)**2)) < .005):
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)
# track data
ee_path.append(np.copy(hand_xyz))
target_path.append(np.copy(target_xyz))
finally:
state=target, target_pos=target_xyz, plot=True)
# returns desired [position, velocity]
if pregenerate_path:
target = path_planner.next_target()
else:
target = path_planner.step(
state=target, target_pos=target_xyz, dt=dt)
# generate an operational space control signal
u = ctrlr.generate(
q=feedback['q'],
dq=feedback['dq'],
target_pos=target[:3], # (x, y, z)
target_vel=target[3:]) # (dx, dy, dz)
# apply the control signal, step the sim forward
interface.send_forces(
u, update_display=True if count % 20 == 0 else False)
# track data
ee_path.append(np.copy(hand_xyz))
target_path.append(np.copy(target_xyz))
count += 1
finally:
# stop and reset the simulation
interface.disconnect()
print('Simulation terminated...')
