def get_target(robot_config):
# pregenerate our path and orientation planners
n_timesteps = 2000
traj_planner = path_planners.BellShaped(error_scale=0.01,
n_timesteps=n_timesteps)
starting_orientation = robot_config.quaternion('EE', feedback['q'])
target_orientation = np.random.random(3)
target_orientation /= np.linalg.norm(target_orientation)
# convert our orientation to a quaternion
target_orientation = [0] + list(target_orientation)
#target_position = [0.4, -0.3, 0.5]
mag = 0.6
target_position = np.random.random(3) * 0.5
target_position = target_position / np.linalg.norm(target_position) * mag
traj_planner.generate_path(position=hand_xyz, target_pos=target_position)
_, orientation_planner = traj_planner.generate_orientation_path(
orientation=starting_orientation,
target_orientation=target_orientation)
return traj_planner, orientation_planner, target_position, target_orientation
interface.set_mocap_xyz('hand', np.array([.2, .4, 1]))
# create our path planner
params = {}
if use_wall_clock:
run_time = 4 # wall clock time to run each trajectory for
params['n_timesteps'] = 100 # time steps each trajectory lasts
time_elapsed = np.copy(run_time)
count = 0
else:
params['error_scale'] = 50
params['n_timesteps'] = 3000 # time steps each trajectory lasts
count = np.copy(params['n_timesteps'])
time_elapsed = 0.0
path_planner = path_planners.BellShaped(**params)
ee_track = []
target_track = []
count = 0
link_name = 'EE'
try:
while True:
if interface.viewer.exit:
glfw.destroy_window(interface.viewer.window)
break
start = timeit.default_timer()
# get arm feedback
feedback = interface.get_feedback()
ctrlr = OSC(
robot_config,
kp=100, # position gain
ko=250, # orientation gain
null_controllers=[damping],
vmax=None, # [m/s, rad/s]
# control all DOF [x, y, z, alpha, beta, gamma]
ctrlr_dof=[True, True, True, True, True, True])
# create our interface
interface = VREP(robot_config, dt=.005)
interface.connect()
# pregenerate our path and orientation planners
n_timesteps = 1000
traj_planner = path_planners.BellShaped(error_scale=50,
n_timesteps=n_timesteps)
feedback = interface.get_feedback()
hand_xyz = robot_config.Tx('EE', feedback['q'])
starting_orientation = robot_config.quaternion('EE', feedback['q'])
target_orientation = np.random.random(3)
target_orientation /= np.linalg.norm(target_orientation)
# convert our orientation to a quaternion
target_orientation = [0] + list(target_orientation)
target_position = [-0.4, -0.3, 0.6]
traj_planner.generate_path(position=hand_xyz,
target_pos=target_position,
n_timesteps=n_timesteps)
orientation_planner = traj_planner.generate_orientation_path(