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(