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
Ejemplo n.º 2
0
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(